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ABSTRACT 
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The major thrust of this work is the development and demonstration of new 
capabilities for the use of small autonomous vehicles in mine countermeasure applications. 
Key to the new capabilities lies in an open architecture tri-level software structure for 
hybrid control, of which this work is the first validated implementation. The two upper 
levels run asynchronously in computing logical operations based on numerical decision 
making, while the lowest, the Execution Level, runs synchronously to maintain stability of 
vehicle motion. The top (Strategic) Level of control uses Prolog as a rule based language 
for the specification of the discrete event system (DES) aspects of the mission. Multiple 
servo controllers are coordinated by the middle (Tactical) Level software in performing the 
mission, while the Execution Level controllers guarantee robust motion stability through 
multiple sliding modes. 

This hardware / software arrangement provides the ability to operate a hybrid 
(mixed discrete state/continuous state) controller for semi-autonomous and autonomous 
vehicles in which the missions imply multiple task robot behavior. This work has defined 
and developed a set of vehicle "primitives", that are a set of stable modular control 
functions unique to a given vehicle's capabilities. It is demonstrated how these can easily 
be combined using rules to specify as simple, or as complex, a mission as desired. 
Completion of a mission is guaranteed through a "complete plan" including time traps and 
error recovery procedures. Experimental results are given illustrating the performance 
attained. 

A particular case of the techniques developed has resulted in a method to navigate 
an AUV in a local area (around a mine-like object) using a profiling sonar sensor for 
position information derived from underwater feature detection. Since sonar image feature 
extraction is necessarily time consuming, a dynamic model of the vehicle response is used 
for control between position updates. A structured formulation of this control / navigation 
method is presented followed by results from in water implementation using the NFS 
Phoenix vehicle and the tri-level software structure described above. 
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I. INTRODUCTION 



At the present time, unmanned underwater vehicle (UUV) activities in military and 
scientific / commercial fields are usually performed by Remotely Operated Vehicles 
(ROV's). These vehicles are tethered to a surface ship or offshore platform by an umbilical 
cable which provides power and control signals. They are employed in the offshore oil and 
gas industries, salvage and recovery, and increasingly, ocean science, as well as in military 
mine countermeasures. Vehicle motion control is provided by a human operator (pilot) on 
the surface, who views the underwater environment through a video camera for short range 
visual feedback. Scanning sonar is often added for longer range information. 

When deep water applications or large horizontal movements of a vehicle are 
necessary, the tether becomes an ever increasing liability. It adds tensile loading on the 
vehicle; often uncertain and time varying, and requires elaborate tether management 
equipment. It is this shortcoming, and the associated costs of the support ship, that has led 
to the development of Autonomous Underwater Vehicles (AUV's). An AUV is 
independent of a tether for power and control, and is free to maneuver more easily over 
larger distances or depths, with little or no direct human supervision. This type of vehicle is 
well suited for performing expensive and monotonous tasks such as ocean water quality, 
bathymetric, and geological survey. AUV's could also be utilized for harbor and 
underwater structural inspection tasks, and most importantly, mine countermeasures and 
neutralization, where there is a potential for loss of life. 

Freedom from the tether is not without cost however, since the ability to 
satisfactorily perform a mission requires a more complex control system. Even though 
underwater communication with the vehicle is possible using acoustics, the transmission 
rates and distances are limited. Real time video feedback to the pilot cannot be 
accomplished. As opposed to control from the surface ship, AUV's have the need for 
increased automated functionality onboard not only for self navigation but also for 
"intelligent" tasks as in fault monitoring, control system reconfiguration, and environmental 
interpretation. 

To implement such a highly automated system, multiple cooperating onboard 
computers or at least a single computer using multiple processes are used. One problem in 
designing and understanding the behavior of such a system is that a meaningful mission for 
an AUV can not be described or modeled using conventional feedback control theory 
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(Friedland, 1986). While this is an integral component of AUV control, a much higher 
level of coordination is required. A typical mission would consist of various phases, such 
as launch, transit, search, recovery, etc., and a method of sequencing these actions must be 
in place. Since the use of a single process/computer to perform all of these tasks would be 
extremely difficult, mutiprocess / mutiprocessor configurations are preferred. It simplifies 
combining the low level, servo control tasks, which must run in real time for stability 
considerations, with high level algorithms designed to manage the mission objectives. It 
eases programming difficulties, even for the simplest mission modification, or software 
changes required for vehicle enhancements. Since sequencing mission phases is inherently 
an asynchronous process, and operates with longer time scales than for the synchronous 
servo controllers, there is no need to run them on the same processor. The natural division 
of synchronous and asynchronous tasks maps well into the separation of symbolic and 
numeric operations and is well suited to multiple computers. Each system can be chosen 
based on their particular attributes with regard to computer languages, operating systems, 
timing constraints, and processor speed. 

Since the vehicle motion exists in a physical world, it can be modeled theoretically 
by a set of differential equations as is well known in describing the behavior and control of 
Dynamic Systems (DCS). On the other hand, the mission sequencing lies in the domain of 
Discrete Event Systems (DES). Manufacturing literature is replete with studies of discrete 
event system control, and (Cassandras, 1993) gives a comprehensive overview, but few 
discuss the integration of DES and DCS. These combined systems are usually referred to 
as "Hybrid" control systems, and will be addressed in detail, as part of this work. 

Early work on this subject was done by (Saridis, 1983), describing a hierarchical 
three level controller consisting of the organization, coordination, and hardware levels. The 
three levels act together using "cognitive and control systems methodologies" with ".. 
control intelligence .. distributed according to the principle of increasing intelligence with 
decreasing precision, evident in all hierarchical management systems". However, this was 
developed in the context of industrial robot manipulators rather than underwater vehicles. 

As applied to underwater vehicles, the three levels have been named in (Byrnes, et. 
al., 1993, Byrnes, et. al.,1996) as the Strategic, Tactical, and Execution levels. The 
Strategic Level is a discrete event system managing the progress of the mission, while the 
Tactical Level coordinates the control modes required, and the Execution Level performs 
the motion control of the vehicle. Byrnes work included a thorough study of both 
backward and forward chaining versions of the "Rational Behavior Model" but was 
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restricted to workstation simulations, not implemented in an actual vehicle operating in the 
underwater environment. 

Other approaches to robot control include the use of subsumption and layered 
control (Brooks, 1986). In principle, the concept is simple and eases the development of 
robot hardware. However, an added coordination layer was found to be necessary for 
AUV's when specific mission objectives have to be met (Bellingham and Consi, 1991). 
Their work has been applied to open water flight control for oceanographic survey 
operations, where only limited robot actions are involved. 

Tri-level control of underwater robots has been studied in France and Portugal. The 
effort in France has focused on the development of a computer-aided design system 
(ORCCAD) (Simon, et. al., 1993) using Esterel - a synchronous language - that encodes 
the mission as a finite state automaton. Also, the PIRAT system is a computer-aided design 
software package for the development, implementation, and verification of servo level 
control laws. This work has been applied to the Vortex vehicle (Perrier, et. al., 1993), 
although the interfacing and integration of these packages with other vehicles could be 
difficult. 

In Portugal, Petri net methodologies at the Strategic Level and gain scheduled 
control at the Execution Level are being employed for the MARIUS vehicle. As part of this 
activity, yet another language - CORAL - is being developed for the real time execution of 
Petri net mission controllers, although, in this case, the cross compiling into C code for 
running on general platforms and interfacing with other vehicles is a promising 
development. 

In spite of these recent research efforts, no unified design methodology for robot 
controllers exists. Implementations on operational underwater vehicles are few and limited 
in scope. In fact, it has not been shown that any one robot control system is uniquely 
superior to another, although some systems appear to be easier to reconfigure than others. 
Therefore, the main purpose of this work is to develop, demonstrate, and validate an open 
architecture, three level control system for an AUV. Using commercially available 
languages, this would allow ease in reconfiguration of control modes, and in particular, the 
examination of robot performance in the context of hovering and local area maneuvering 
around a target. The importance of this work is the use of an existing Al computer language 
known as Prolog, as opposed to a special purpose language such as Esterel. Prolog is 
convenient for Strategic Level sequencing of the mission phases, and allows the use of the 
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full powers of the Prolog inference engine. It also provides a natural link between symbolic 
operations and the C language numerical computations in the Execution Level. 

These control concepts have been validated on a small underwater vehicle (6 ft in 
length), which possesses both fins, cross-body thrusters, and rear propulsors for motion 
control. In the majority of this work, the thrusters and rear propulsors are used for 
movement in a small area. The vehicle is able to maneuver in many different directions and 
if needed, backwards. With this type of general motion capability, there is no fixed speed 
and direction of motion, and since the motion is limited, the vehicle rarely reaches a steady 
state condition. The vehicle is subject to highly non-linear effects from quadratic lift and 
drag forces, (Fossen, 1991, Yuh, 1990, Sarpkaya and Issacson, 1981) so that linear 
modeling and control techniques are not suitable. One approach in nonlinear control is to 
linearize about some nominal operating point. When operating points change slowly, gain 
scheduling techniques are often employed (Healey, et. al., 1995). In fact, linearization 
about a constant forward speed is the usual technique used for submarine and torpedo 
control design (Milliken, 1984, Lindgren, et al., 1967). However, in the cases studied in 
this work, slowly changing operating points do not exist, and linearization about such 
points is not possible. For these systems, non-linear techniques are required to ensure high 
performance control and stability. 

For the Execution Level, application of the sliding mode methodology (SMC) is 
used throughout this work. It can deal with non-linear dynamics directly and is effective 
against parametric uncertainties and unmodeled disturbances. A tutorial covering general 
concepts is given in (Decarlo, et. al., 1988). Simplified applications to ROV's and AUV's 
are given in (Yoerger and Slotine, 1985), (Yoerger, et. al., 1986). A multivariable sliding 
mode autopilot for AUV's using decoupled modeling for speed, steering and diving was 
described by (Healey and Lienard,1993). Dynamic positioning of ROV's using sliding 
modes is described in (Fossen, 1991) and (Fossen and Sagatun, 1991). 

The objectives of this work separate into five distinct parts. 

1. Chapter II covers the general theory of vehicle control using Sliding 
Modes. A 6 DOF mathematical model of the vehicle and controller is given 
together with 3-D command generators for precise velocity and positioning 
maneuvers. Also included is a robustness analysis of the controller design 
validated by computer simulation results. 
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2. Robot control of the NFS Phoenix vehicle is discussed in Chapter III, 
starting with an overview of robot control structures in general from various 
other efforts throughout the world. Details of the computer hardware and 
software systems used for mission control are provided. Results from in-water 
experiments show the effectiveness of this control method, and a short critique 
of the control architecture is presented at the end. 

3. In Chapter IV, an in-depth discussion and description of the acoustic 
sensors used by the vehicle for navigation and environmental assessment is 
given. 

4. Chapter V gives in-water experimental results and performance evaluations 
of sliding mode control for the Phoenix. Results of submergence, rotation, and 
longitudinal motion control experiments are presented which include Kalman 
filters necessary to the successful implementation of the respective control 
designs. Results of coordinated maneuvers using combinations of 
submergence and rotation control are presented. 

5. Local area maneuvering of the Phoenix with sonar is outlined in Chapter 
VI. This chapter contains a description of experiments conducted with sonar 
control algorithms for target recognition, relative range and bearing 
calculations. Two algorithms have been applied to this problem together with 
simulation and complete verification for each. In-water results using one of the 
algorithms are presented together with a performance evaluation. 

A summary of the dissertation is given in Chapter VII, which contains concluding 
comments, remaining issues, and recommendations for future work. An extensive section 
of appendices is presented at the end of this work to provide the reader with implementation 
details and software that was not deemed appropriate for the body of the text. 
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II. GENERAL THEORY OF AUV MOTION CONTROL 



This chapter discusses the foundation and design of a MIMO (Multiple 
Input/Multiple Output) Sliding Mode controller for an (AUV) in performing precision 
tracking to command signals. Precision control is needed for maneuvers such as automatic 
docking, recovery, and submerged object inspection / identification. Whenever an 
underwater vehicle maneuvers in the surrounding water column, it is inevitable that reaction 
forces, caused by time varying changes to the pressure distribution around the vehicle 
body, are generated. These forces arise from hydrostatic buoyancy as well as from the 
relative velocity and acceleration of motion. Hydrodynamic forces and moments are usually 
expressed in terms of lift, drag, and added mass components modeled with assumed 
constant coefficients. Since the hydrodynamic coefficients are only estimates, often based 
on poor representations of reality at best, large parameter uncertainty exists. 

The system to be controlled is highly nonlinear and coupled. While tracking control 
could be implemented using a variety of techniques that have been proposed recently, it has 
been confirmed during the course of this research that control using sliding modes results 
in high gain robust controllers that are easily tuned and modified. This particular attribute is 
beneficial because it leads to the notion that control law adaptivity could be readily 
accomplished. 

In this chapter the NPS Phoenix AUV is used as the basis for the development of 
the control theory and is assumed to have 8 independently controllable fins, two stern 
rudders, two stem planes, two bow rudders, and two bow planes. Propulsion is provided 
by four cross body thrusters, two lateral and two vertical, with two rear screws. The 
computer simulation model so developed is based on non-linear equations of motion for 
this vehicle. Section A describes the vehicle model with a controller design using sliding 
modes. A robustness analysis of the control design is given in Section B. A design 
methodology for robustness is presented which accounts for parametric discrepancies 
between the dynamic model used for controller design and the actual vehicle. Section C 
presents the specific sliding mode control implementation for the NPS Phoenix vehicle. 
Discussed in Section D is control allocation among the various actuators available to the 
vehicle. Since some vehicles, including the Phoenix, are "over-actuated", methods to 
allocate the control effort are of primary importance and several suggestions to handle this 
are presented. Two computer simulations of the performance of the control design are 
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presented in Section E. One case uses all available actuators (thrusters, fins, screws), while 
the second uses only fins and the rear screws for control. The simulation uses the complete 
set of equations of motion, although the particular cases presented here are restricted to the 
horizontal plane. Command generators for position, velocity, and acceleration as a function 
of time along a specified trajectory are developed and discussed in the performance of 
precision tracking control. The final section gives conclusions and recommendations for 
future work. 

A . SLIDING MODE CONTROL AND MODEL BASED CONTROL LAW 

DESIGN 

This section outlines a general theory and the design of a MIMO Sliding Mode 
Controller for an underwater vehicle. This method provides robust control of systems with 
nonlinear dynamics, parameter uncertainties, and disturbances. The main advantage of this 
approach is the use of a switching term in the control law which drives the plant's state 
trajectory onto a specified surface, known as the sliding surface. "Chattering" of the control 
input caused by high gain in the switching term, is reduced by the introduction of a 
boundary layer around the sliding surface. The effect of this boundary layer, is to lower the 
feedback gain when small errors exist, resulting in smooth control. This technique is 
particularly useful when feedback signals are noisy. Stability is analyzed using standard 
Lyapunov methodology. 

1 . Vehicle Mathematical Modeling 

The mathematical model of a system describes it's dynamic behavior, and is usually 
embodied in a set of differential equations. It forms the basis by which stability of feedback 
control laws can be assessed. It is also critical to the design of predictive and other more 
advanced model based controllers, of which, those using sliding modes, form one 
example. It follows that a mathematical model of an AUV must be established if advanced 
model based methods are to be developed. At that point, an array of analytical and 
computer tools can be used for analysis and synthesis purposes. 

An underwater vehicle can be assumed to be a rigid body, capable of adopting any 
position and orientation in the water column. However, in contrast to high speed aircraft 
and spacecraft, significant departures from a predominantly horizontal attitude are 
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uncommon. Three independent position and three Euler angles are required for describing 
the location and attitude of the vehicle. For convenience, three coordinate frames are used. 
One, denoted as the body-fixed frame (O), is rigidly fixed to and rotates with the vehicle, 
the second, is a global, earth-fixed frame (G) taken as the inertial reference, and used for 
local area navigation. A third reference frame (C), parallel to the inertial reference, moves 
with the local fluid particles at a constant velocity, u^, (the local current) where 



Rotational currents and fluid accelerations are assumed to be zero in this work. The three 
reference frames and their relation to each other are shown in Figure 2.1 which show the 
body-fixed frame in an arbitrary location and the centers of mass and buoyancy described 
by the position vectors and respectively. The vehicle motion, expressed in the 

body-fixed frame, is defined in terms of the six components of the velocity vector x(t), 
where 



A distinction is made in this definition between absolute velocity of the rigid-body 
and it's velocity relative to the surrounding water column. Typically, the vector x{t) is 
viewed to be the absolute velocity of a rigid-body expressed in the rotating, body-fixed 
frame as described by (Fossen, 1991). However, this formulation introduces difficulties if 
a fluid current is present, since the hydrodynamic lift and drag terms are functions of the 
velocity of the fluid particles relative to the vehicle, rather than it’s absolute velocity. The 
relative velocity formulation overcomes this difficulty. With this definition, the principle of 
relative velocities can be used to obtain the absolute velocity of the body given the three 
reference frames. The position of the vehicle in the global reference frame is given by 



An Euler angle transformation (the definition and use of Euler angle transformations is well 



vehicle relative velocity components, x(t), to the rate of change of global position, z(t). 
The global velocity of the vehicle can now be expressed as 




x{t) = (»(/) v(r) w{t) pit) q{t) r{t)) ^ . 



( 2 . 1 ) 



zit) = {Xit) Y{t) Z{t) <p(t) 6{t) wit))\ 



( 2 . 2 ) 



known and described in standard texts on the dynamics of rigid-bodies) is used to relate the 
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z{t) = h{z{t))x{t) + M,(r), 



(2.3) 



where h{z{t)) is the 6x6 transformation matrix as a function of the Euler angles 0(r), 
d(t), y/(0. (spin, elevation, and azimuth respectively), given by 



Hz(t)) = 



T,(z(t)) 0 

0 TMt)) 



where 



T,{z{t)) = T{y/fT(efT((t>)\ 



and 





' 1 


0 


0 




cosG 


0 


-sind 




cosy/ 


sin y/ 


o' 


= 


0 


cos(p 


sin(p 


; ne) = 


0 


I 


0 


; = 


—sin y/ 


cosy/ 


0 




_0 


—sin(p 


cos(p 




sinO 


0 


cosd 




0 


0 


]_ 



The matrix transformations for the translational, T^{z{t)), and rotational, T^{z{t)), velocity 
components can also be expressed as 






cosy/cosd 
siny/cosd 
-sin 6 



cosy/sindsin(p — sin\i/cos<p 
siny/sindsincl) + cosy/cos(p 
cosOsincp 



cos\{/sindcos(l) 4 - sinxi/sin^ 
sin\f/sindcos(p - cosy/sin(l) , 
cos 6 cos (p 



and 



TMO) = 



1 

0 

0 



sin<t>tan6 

cos(p 

sin(j) / cos 6 



cos(ptanB 
-sin 6 
cos(j) / cosd 



Note that /i(z(0) is not an orthogonal matrix, and h{z{l))~‘ * /i(z(r))^. 

With the coordinate frames defined, Newton's second law of motion may be used 
to formulate a dynamic model of the system. The dynamics of the AUV can be described 
by a set of six non-linear, coupled, second order differential equations with constant 
coefficients. For a submerged rigid-body, the equations of motion formulated in a body- 
fixed reference frame with an arbitrary origin and constant mass and inertia is given by 
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fn(v„ + «„xv„ + Q)„xrc + 0)„x{a)„xrc)) = F„ 

+ f^r^xv^, + mr^x{(o„xv^,) = M„ 

where the first vector equation represents the translational motion, while the second 
describes the rotational motion. v„, and are defined by 

V„ = (m V w)’’ 

= (p 9 rY 

~ (-^C ^g) ’ 



m is the vehicle mass, and the inertia tensor with respect to the body-fixed reference is 



I 



o 



^x\ Av A': ■ 

-A. -A. 



and M„ represent the forces and moments respectively, derived from gravitational 

effects of weight and buoyancy, hydrodynamic lift and drag terms, which are functions of 
a set of hydrodynamic coefficients, and the relative velocities defined by x(t). These terms 
also include the effects of hydrodynamic added mass, disturbance forces/moments, and any 
control inputs from thrusters and control surfaces, and each element can be described by 

F„ = (X, y. zj 



where X„, and are the surge, sway and heave forces, and K„, M„, are the roll, 
pitch, and yaw moments. 

In matrix form, the dynamics model may be represented as 

Mx{t) = f (x{t),z(t),c) + g(x(t),z(t))u(t), (2.4) 
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where M is the 6x6 mass matrix represented by 



m - 


0 


0 


0 


m -Y, 


0 


0 


0 


m - Z^, 


0 


-{mzc + K,) 


myc 


mzc 


0 


-{mxc + M„. 


-mya 


mxc - 7/, 


0 



0 mzc -myc 

-[mzc + 0 mx^ - y, 

myc -(mxc + Z J 0 

L - Kr -L -(/« + f^r) 

-L - M. -A, 

+ w,) - n. 



which includes the hydrodynamic added mass and off diagonal terms. The vector 
f {x{t),z{t),c) is the 6x7 set of forces/moments from centrifugal and Coriolis effects, 
gravitation and hydrodynamic lift and drag terms, which are functions of a set of 
hydrodynamic coefficients, c, as described earlier. u{t) is the input control vector with 
dimension 6xnt, of the form 

u(t) = Uj{t) u^(t) ■■■ ujof, 

where m is the number of control inputs and is determined by the specific vehicle design. 
The input gain matrix, g(jc(0.z(0). contains coefficients that describe the effectiveness of 
each control input on the vehicle motion, and is speed dependent if control surfaces are 
used. For more detail refer to (Yuh, 1990, Fossen, 1991, Healey, 1993), and for specifics 
of the NFS Phoenix design, see Appendix A. 

2 . Sliding Mode Control Design 

Now that the dynamic model of the vehicle has been formulated, the sliding mode 
control law can be designed. With the exception of cruising vehicles in flight control modes 
(where the main control modes are for vehicle speed, heading, and depth), it is usually 
desired to control the motion of an underwater vehicle relative to an inertial reference, not 
relative to the water column. Therefore, the appropriate error vector is defined in terms of 
global coordinates as 
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e 



(2.5) 



z(t) 




'icon,y) 




'z{t) 


_z{t)_ 




Za,JO_ 




_z(t)_ 



The subscript com refers to the commanded value of the position or velocity in question, 
where commanded time variations of states must be consistent with vehicle physical 
capabilities and usually come from a separate path planning algorithm called a "command 
generator". Position, velocity, and acceleration profiles are ideally kinematically consistent, 
and continuous with the possible exception of acceleration. One method to generate 
position, velocity, and acceleration profiles is presented in detail in Appendix B. 

Since Eqn. (2.1) in terms relative velocities, and Eqn. (2.5) consists of the global 
quantities to be tracked, modification of the dynamics equation, (2.4), is needed. By 
differentiating Eqn. (2.3), an expression for x(t) in terms of z(t), z{t), and the current 

is given by 



x{t) = h{z{t)Y'z{t) - h{z{t)y‘h{z{t))h{z{t)y‘{i{t) - (2.6) 

Substituting Eqn. (2.6) into (2.4), rearranging and dropping the "function of" notation for 
clarity, the dynamic equations of motion are now compatible with the definition of the 
tracking errors 



z{t) = hM~‘{f + gu{t)) + hh~'{z(t) - mJ. (2.7) 

The objective of a controller is to force the tracking error to zero as time increases. 
Using sliding mode methods (DeCarlo, et. al., 1988, Yoeger and Slotine, 1985), a set of 
sliding surfaces, <7(z(/)), is defined as 



where 



<T(z(/)) = [S, S, 



5 .] 



i(0 

z{t) , 
jz(t)dt 



a{z{t)) € S„ S„ S, e 



( 2 . 8 ) 
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and for an underwater vehicle with 6 degrees of freedom, /i = 6. If < n, the system is 
under actuated and only a subset of the system modes will be controllable. The reader is 
referred to (DeCarlo, et. al., 1988) for a discussion on switching surfaces for a system 
with n states and m actuators. 

In spite of the fact that normally there are the same number of sliding surfaces as 
control inputs, for the m inputs defined, there are only 6 independent combinations 
corresponding to the six independent degrees-of-freedom of the vehicle. It follows that six 
sliding surfaces are required for the description of six independent sliding modes rather 
than ten. 

An integral term is included to remove any steady-state position errors that may 
arise from discrepancies between the assumed current magnitudes and the actual, as well as 
the presence of unknown disturbances that are not modeled. The elements of S,, S^, and 
Sj can be selected to provide the desired performance of the closed loop system. Stable 
tracking behavior is achieved, if the condition; 



together with 
will also imply 



Urn > 0, 

/^oo 

lim <t(z(/)) — > 0, 

f— >oo 

z(r) — > 0 as r — > 



so that the elements of Sj , S 2 , and Sj are chosen to provide stable polynomial functions 
of state errors. Also, if 5/ = /, there is no loss of generality. Conditions under which 
is always decreasing can be established using Lyapunov theory by defining V{g) 
as 



V(a) = ^o-^(z(r))*a(z(r)) > 0 1 > 0, (2.9) 

such that V{0) = 0 and is always increasing with cr(z(0). while 

V{t) = <T^(z(/))*<T(z(/)) < 0 t > 0. (2.10) 
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Global asymptotic stability is then guaranteed if V(t) is positive definite while V(t) is 
negative definite. The quadratic nature of (2.9) assures the positive definiteness of V(r), 
while negative definiteness of V{t) may be obtained by a criterion satisfied by 

= -r],sgn{G,{im i = 1...6, (2.11) 

where each 77 , is a positive scalar. The positive definiteness of V(r) and the negative 
definiteness of V(r), implies that given any initial condition, o{z{0)), o{z{t)) will remain 
bounded such that V{o{z{t)) < V{a{z{0)). 

Since sgn{G^{z{t))) is discontinuous across g(z(0) = 0, undesirable switch 

chattering can occur, and can be alleviated by the use of a "boundary layer" around 
a(z(t)) = 0. Therefore, instead of using a sgn function, a continuous one could be used 
such that 



sgn(G^(z(t))) 



sat(G,(z(t)) / (f>i) = < 



G,(z(t)) /</),■ 



if |<T,(z(0)| > 0, 

otherwise . 



Another approach is to simply use the continuous function tanh{o{z{t))). All three 
functions are shown in Figure 2.2. Substituting the definition of sat into Eqn. (2.1 1) and 
noting Eqn. (2.8), it can be written in a more compact form as 

G{z{t)) = lit) + s,i{t) + S,m = -F{o{z{t)\ 0), (2.12) 



where 



'ri,sat{G,{z{t))/ 

ri,sat{G2(z(t)) / (t>2) 



F{o{z{t)), 0) = < 



ri,sat{G^{z{t))l 
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Substituting the dynamics equation, (2.7), into the definition of the sliding surface 
(2.12), and again dropping the "function of" notation yields 

- hM~‘f - hM''gu(t) - hh~'{z{t) - «,) 

+ S^zit) + Sjl{t) = -F((T, ^) , 

and after rearranging, 
gu{t) = 

- / - Mh-'hh-’[i{t) - + Mh-'S,tU) 

+ Mh~‘SjZ(t) + Mh~‘F{a, (f>) . 

Since the matrices M, /, g, h, and are uncertain in general, the control solution, 
u{t), is formulated using their estimates, denoted as M , /, g, h, and u^. The control 
vector can be split in three parts 

//(/) = U,{t) + M^(/) + M^(/), 

where 

u,{t) = d[Mh-‘z,„„{t) -/) (2.13) 

contains the feed-forward compensation for acceleration requirements, 

M^(/) = - z ) -H S_,z(oj (2.14) 

contains feed-forward and feedback compensation for velocity, and finally 

«,(/) = GMh'{S,z + F(<t,^)) (2.15) 
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contains feed-forward and feedback compensation for the position errors including the 
switching term based on sliding surface values. G is defined as the pseudo-inverse of g 
and in the case of an over-actuated vehicle, is an mx6 matrix, where m > 6, an 
optimization criterion is needed for uniqueness (See Appendix C). 

In order to implement the control solution defined above, both h~' and G must 
exist. The condition for existence of h~' is that the elevation angle, 6, does not become 
± 90°, and for an underwater vehicle, this is unlikely if it is properly designed and 
controlled. As for the existence of G, this depends on the actuator arrangement of the 
particular vehicle of interest. If certain modes are not directly controllable, G is rank 
deficient and modifications to the control solution will be required, and is the topic of 
Section C. 

3 . A Comment on Full State Feedback 

The analysis presented requires full state feedback, and in many cases this is not 
unreasonable. Current inertial navigation systems provide all six rotational states. Doppler 
sonar gives both speed over the water as well as ground speed, and Long Baseline acoustic 
positioning systems provide X and Y, while pressure sensors give a measurement of Z. 
The use of extended Kalman filters for navigation can provide all twelve states. 

B. ROBUSTNESS ANALYSIS AND ASSESSMENT OF 

UNCERTAINTY 

This section presents a robustness analysis of the sliding mode controller 
algorithms previously developed. Accurate modeling of "real" systems can be very difficult 
especially underwater vehicles. Determination of the mass properties, hydrodynamic 
coefficients and the control gains for the various actuators can be very time consuming and 
in some cases impractical. It is a relatively simple task to exactly measure the system's 
"dry" mass parameters but more difficult to determine the added mass coefficients. The 
force characteristics of thrusters can be accurately measured as a separate unit but when 
incorporated into the vehicle, it's behavior becomes dependent on vehicle motion, 
orientation, and any currents that may be present. Even if these parameters are carefully 
identified initially, over time, mechanical wear or environmental changes can cause 
inaccuracies. Since controller designs based on inaccurate models can cause poor 
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performance or even instability, provisions for this must be incorporated into the design. 
Sliding mode control can, so long as the extent of the parameter imprecision is known and 
bounded, provide a systematic approach to this problem. The following analysis deals only 
with structured (or parametric) uncertainties present in the model, and assumes no 
unstructured uncertainties involving unmodeled dynamics are present. 

Recalling the dynamics model in Section A, 

z{t) = hM~’{f + gu{t)) + hh'‘[z{t) - «,), (2.16) 

the forces, /, the input gain matrix, g, the mass matrix M , and the current are not 

exactly known. Therefore, estimates of these quantities must be made to formulate the 
control solution and are denoted as /, g, M , and u^., which are related to their exact 

values by; 



|/, - f. 



< F: 



g = 



f^l.J 



< D. 






M = A^M 






< D, 



M,, 






< 



Each F,, , and U^. represent bounded maximums on the estimation errors for 

any maneuver. Uncertainty in the transformation matrix, h , is assumed small and will not 
be included in the analysis. 

With the above defined, and returning to the definition of the sliding surface 

a(z(t)) = S,i{t) + S,z(t) + S,jz(t)dt, (2.17) 



it's derivative 



&(z(t)) = S,f(/) + S,i(t) + S,z{t) (2.18) 
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and the condition for global asymptotic stability 



Oi(z(t)) = -ri,(a,(z(t))) i = 1 ... 6. (2.19) 

Dropping the "function of" notation, and recalling that h = ^, the sliding mode control 
law from Section A is 

u = - hM~'f + hh~‘[u^ “ + Tj.*sgn(<j)y 

(2.20) 

where T].*sgn(CF) is functionally equivalent to F(<r, <f>), and the notation refers to 
element by element multiplication. 

For the purposes of stability robustness analysis, the sat function used previously 
in the control law is replaced with a sgti function, since pure switching does not occur 
within the boundary layer, - < O’, < +0,, the effectiveness of the gain, 7]-, is 

reduced. In order to simplify further discussion, the matrix S/ can be taken to be the 
identity matrix without loss of generality. Further, 5^ and 5, are assumed to be diagonal 
matrices with separate bandwidth parameters for each mode. It follows that 

f + gGMh-'{z,„„, - hM-‘f + hh-‘{u^ - z) 

. . . (2.21) 
+ S^z + SjZ + T].*sgn(<j)jj + hh '[z -«,.) + S^z + SjZ 

which describes the dynamics of the closed loop sliding surface. For clarity (2.21) can be 
separated into n scalar equations written as 

(t(0, = OC(t), + /5(0/ V5«(<7(0,) 2 = (2.22) 

where OC^(t) and /?,.(/) are time dependent scalar quantities for each (T,(/) whose output 
will be bounded stable only if 

n. > l^3r'(')||Ja, (/)|. (2.23) 
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where ||<^,(f)lL denotes the infinity norm of CC,(t), also defined as 
max\CX^{t'j^ V Selecting each rj, according to Eqn. (2.23) will overcome any 

destabilizing effects due uncertainty in either Af, /, g, ox and provide a robust 
controller design. Although the uncertainties were originally defined in terms of maximum 
bounds, the complexity of (2.21) prevents their direct use in the analysis. Since the terms 
(X^{t) and )3,(r) are not only functions of the parameter estimates, but are also functions of 

the time dependent states, simulations for each control maneuver could be performed to 
determine lower bounds for each 77,. Naturally, actuator saturation may limit the ability of 

the control to perform its task, while guaranteeing stability, and this issue may also be 
studied by simulation. 

1 . A One-Dimensional Example 

To more clearly illustrate the robustness analysis techniques outlined above, a scalar 
example will be given utilizing the maximum bounds formulation for parametric 
uncertainty. The analysis is an extension of the results from (Slotine and Li, 1991). 

A simplified model of an underwater vehicle operating in the surge direction, x, 
can be written as 



where u is the control input. M is the mass of the vehicle, including the added mass 
associated with motion in the surge direction, / , the hydrodynamic drag force, and g is 
the input gain and is assumed to be positive. Estimation errors of the mass and input gain 
can be described multiplicatively by 



Mx = f + gu. 



(2.24) 



1 < M. 

jd ~ M 



^ fj- ; > 1, 



(2.25) 




y > 1, 



(2.26) 



7 g 
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while the bound on the drag force uncertainty may be formulated as 




(2.27) 



and F is the infinity norm of ^/ - /j, since / is a function of the time-varying state and 
can be either positive or negative. 



Defining the sliding surface as 



(7 = X + Xx, 



and it's derivative 



(2.28) 



& = X + AJc, 



(2.29) 



the resulting control law in terms of the estimates is 






u = 



M 

g 



f 

Xcm ~ ^ + Vsgn{<y) 

M 



\ 



(2.30) 



Substituting (2.30) into (2.24) provides the closed loop system dynamics 



/ Mg 

X = — + — - 

M Mg 









f 

^con. " + rjsgn(<7) 

M 



(2.31) 



and using Eqn. (2.29), the condition for global asymptotic stability becomes 
do = 



f ^ ^ 




1 - 


Mg] 




^ 1 


\ 


M M g M 




V 


Mg) 




Y com j 


/ 



~r(\o\ < 0. 

M g 



(2.32) 
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By choosing the value of r\ to be large enough, the condition for stability will be satisfied 
despite the parametric uncertainty. After several steps of algebraic manipulation of Eqn. 
(2.32), and noting that ji~‘ < MM~’ < fi and / = / + (/-/j,the minimum value of 

the switching gain, rj, that will satisfy the stability requirement is 



n ^ ^ + ^ K' - y)l + l^‘y- 



(2.33) 



Inspection of (2.33) reveals that the required value of 77 for stability increases with the 
degree of parametric uncertainty. On the other hand, if a "perfect" system model is 
available, (2.33) reduces to 



Tf > 0, 

Although the robustness analysis relied on the use of a sgn function as the 
switching term, in practice the sat function is used to alleviate undesirable actuator chatter 
which gives rise to lower tracking precision. Since this approach does not provide fast 
switching within the boundary layer, tracking performance will degrade. Therefore a 
design trade-off between tracking performance and control activity exists. The degree of 
parameter uncertainty also plays a role in performance issues. The greater the modeling 
uncertainties, the greater the required control effort, which can cause actuator saturation and 
possibly system instability. 

Use of a state observer can reduce the performance and robustness of the control 
(Cristi, et al., 1990(a), Cristi, et al., 1990(b)) show that errors may still be bounded, but 
robustness guarantees can not be proven to be superior than those using linear LQG/LTR 
techniques. 

C . SLIDING MODE CONTROL IMPLEMENTATION FOR THE NFS 
PHOENIX VEHICLE 

In order to verify the above theory, simulations have been conducted using a 
mathematical model of the NPS Phoenix vehicle, developed amongst other reasons for the 
purpose of experimental validation of the control concepts contained in this dissertation. 
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While details of the vehicle are given later in Chapter IH, we should note for now that it has 
four cross-body thrusters and eight independently controllable fins, as well as twin rear 
propulsion screws to effect its motion control. 

For the NFS Phoenix vehicle, the input control vector is thus defined as 



which contains ten independent actuators. The inputs ^hr > ^bp > 5^, 6 ^ 1 , are the bow 

rudder, bow plane, stem rudder, and stern plane surface deflections respectively (±0.4 
radian maximum, deflection). F,, and are the forces due to the left and right rear screws 
(±5 lb. maximum force). F^,, and are the bow and stern lateral thmster forces, while 
the bow and stern vertical thruster forces are defined as Fb. and F,^,, (±2 lb. maximum 
force), all four of which are through-hull and can operate bi-directionally. All control inputs 
are shown pictorially in Figures 2.3 and 2.4. Only four fin commands are needed since 
they act in coupled pairs, and the simulation is based on the assumed numerical values of 
hydrodynamic and thruster coefficients given in Appendix A. 

With the defined control inputs, the resulting 6 x 70 input gain matrix for this 
actuator configuration is 



From this, it can be seen that g does not have full rank since the roll mode is not directly 
controllable using the inputs available. In this situation, the inverse of g does not exist, 
and no direct control solution can be obtained. The uncontrollable mode is the roll motion 
and must be passively stable for a properly designed underwater vehicle. With this 
assumption, the input gain matrix can be redefined by re-ordering the state vector and 
separating the roll equation of motion. This results in a new state vector 




(2.34) 



0 0 0 0 1 1 0 0 0 0 



uWdbr 0 u\u\Yj^^ 0 0 0 0 1 0 1 

0 M^jbp 0 0 0 10 10 

0 0 0 0 0 0 0 0 0 0 



0 M^jbp 0 0 0 -x^^, 0 -x„, 0 

M^jbr 0 0 -y,, -y„ 0 x^„ 0 x^„_ 



x\t) = (m(0 v{t) w{t) q{t) r(r) p{t)) 



(2.35) 
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and 



Z*(/) = (X(/) Y{t) Z{t) 6{t) xir{t) m)\ (2.36) 

where the corresponding input gain matrix is 
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which has full rank and is invertable, such that g G = I . Rearrangement of the state 
vector not only impacts g, but the mass and transformation matrices must also be altered 
resulting in the following 



'm - 
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mZa 


->nyo 
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in - Y^. 
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mx^ - y. 
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-[inx^ + Z^) 


0 


myc 


mza 


0 


-{>nxa + M„) 


L - 
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-{mzc + K,) 


>nyc 


-K 


+ K) 





and 






T;(z(t)) 0 

0 Tlizit))' 
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where 






cosy/cosd cos\f/sindsin(p - siny/cosip cos\j/sindcos<p + siny/sin(p 
siny/cosd siny/siii6sin<p + cosy/cos(p sinyfsin6cos<p - cosyfsirKp , 
—sinO cos6sin(p cos6cos(p 



and 



Kizit)) 



cos<p —sin<p 0 

sin(p I cosQ cos(p / cosQ 0 , 
sitKptand cos(ptan9 I 



where the superscript denotes the matrices of the rearranged system. Eqns. (2.13) 
through (2.15) must now use the redefined matrices and vectors to compute the control 
solution, 1/(0. 

As an aid to understanding this procedure and verifying the stability of the result, 
the following definitions and remarks are given: 

Definition: A system, Z, 

I: x(t) = f(x{t)) + g(xit)Mt), x{t) e u(t) e m>n, and /, are 

smooth vector fields on SR" and g are smooth matrix functions e SR"’"'" is said to be 
directly controllable if g has rank of n , and a unique G exists such that gG = / e SR"^" 
V r:[o,~). 



Remark 1: It follows that Z, a directly controllable system may be globally 
asymptotically stabilized by the control u{t) - G{x{t)){-f{x{t)) - rj.*sgn(x(t))). This 
can be proven using the sliding surfaces <T = x, and rj >0. 

Lemma: For the system, Z , where rank ^ is < « , Z can be separated into Z, and 
where Zj is a directly controllable subsystem and Z^ is unforced by u(t). 

Proof: If g is rank deficient, a reordering of the state vector can be performed with 

components 
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X 




X 



X 



such that if the rank of g is p, with p<n, g e SR"’"" is then written as 



and since rank rank{gj) = p, then a G can always be found so that g,G = I e 
Z; is then directly controllable, and is unforced by «(/). 



X2{t) = f2[x,{t),X2{t)] 

Lemma: The system L with rank(gj) = p<n may be globally ultimately stabilized if its 
subsystem, is passively stable, i.e. 



Proof: Using the concept of passive stability, we define Lyapunov candidate functions, 
V/(at,(/)) = 0.5xJ{t)x,{t), with V,(Arj(r)) = O.Sx^iOx^it). For the entire state, jc(/), 

the Lyapunov function is V(/), where 

V(t) = V,(0 + V,(/). 

It follows that V < 0 if V, <0, and negative definite for all t, by virtue of 

the sliding mode control design for «(/) remembering that both x,(t) and x^it) are known. 
Thus t implies that Xj(t)^0- is <0 for all t>0 iff, 

x](t)x 2 (t) < 0, V /:[0,°o) leading to the sufficient condition. 




/ = [ x[(T)/_,(jr/(T),Ar,(T))JT < 0, r^oo. 
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The integral /, is a measure of the net energy into the subsystem, induced by Xj(t), 
and dissipated by x,(/). 

Remark: Ultimate stabilization of X,, and thus X can be proved if there exists a such 
that 7(^2, «») < 0 for all t>t 2 . 

Remark: Passive stability for X^ also implies that in tracking problems where the state 
x,(t) is driven to the bounded value such that the error x,(t) — > (7, r -» x^it) 

will be bounded. 

(Note that x, = {u v w q r)^ and = /? for the control design implementation studied 
here.) 

D . CONTROL ALLOCATION 

One method to solve the inverse of g(x(t),z(t)) is to use the minimum norm 
solution or weighted minimum norm solution outlined in detail in Appendix C. Use of the 
weighted minimum norm will be the most appropriate since certain actuators will have large 
or small effects depending on the vehicle speed and orientation with respect to a 
commanded path. Equal weighting of all inputs will cause certain actuators to saturate 
under different operating conditions. Since a continuous control over the entire speed range 
is desired, the elements of the weighting matrix, W , should be designed as functions of 
vehicle forward speed, and perhaps the magnitude and direction of any currents present, 
scaled by the maximum level of individual actuators. For the simulations to follow, the 
weighting matrix is diagonal and represented by 
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V, 0 0 0 

^hr 

0 w. 0 0 

^hp 

0 0 w. 0 

0 0 0 w. 

0 0 0 0 

0 0 0 0 

0 0 0 0 

0 0 0 0 

0 0 0 0 

0 0 0 0 



0 

0 

0 

0 

Wr 

^ts 

0 

0 

0 

0 

0 



0 0 0 0 O' 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

w. 0 0 0 0 

0 Wp 0 0 0 , 

0 0 w. 0 0 

^hU 

0 0 0 w. 0 

^SVf 

0 0 0 Owp 



where each vv, corresponds to each actuator given in Eqn. (2.34). Using this means 
continuous control over the entire speed range can accomplished. For example, the control 
surfaces should be given the highest weighting at cruising speed, while the thrusters should 
dominate when the vehicle is maneuvering at low speed or during hovering operations. 
Manipulation of the weights either as a function of state or by a pre-defined plan is a 
convenient way to reconfigure control systems while maintaining stability. 



E. SIMULATION RESULTS 



A two dimensional tracking simulation is now presented which demonstrates the 
performance of the sliding mode controller. Motion of the vehicle is restricted to the 
horizontal plane with the desired trajectory shown in Figure 2.5, which is composed of two 
straight segments 30 feet long and a quarter circle of radius 60 feet. Using the command 
generator algorithms outlined in Appendix B, motion profiles for position, velocity, and 
acceleration of the vehicle surge motion have been specified. For the maneuver, the 
trajectory arc length is 154.24 feet with a specified maximum velocity, of 1.0 ft / sec 
and maximum acceleration, , of 0.035 ft / sec^ . Since the trajectory involves a turn of 
90 degrees, the global position and heading commands must be kinematically consistent, 
and this can be achieved by using Eqn. (2.3). Assuming no current, the global commands 
can be derived from 
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Zc„m(0 = ^h{z{t))x^„„Xi)dt 
4™(0 = h{z{t))x^,,^{t) 

ZcoJO = Hz{t))x^.„„Xt) + h{z{t))x^„Jt) 



and for each segment, 



^con.(0 = 



0 

0 

0 

0 

0 



0 < t < 58.7 sec. ; 0 < s < 30.0ft. 



Xc„n,iO = 



'^coJO 

0 

0 

fcoJO 

0 

0 



58.7 < t < 153.0 sec. ; 30.0 < s < 124.24 ft. 



XcoJO = 



l*con,(0 

0 

0 

0 

0 

0 



153.0 < t < 21 1.2 sec. ; 124.24 < s < 154.24 ft. 



and 






R 



58.7 < t < 153.0 sec.. 
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where R = 60 ft., the radius of the turn. From this, the global motion command vectors 



are 



'X{t) 




1 

1 




'X(t)' 


Y{t) 


= 


m 


Zc„n,(0 = 


m 




com 


j{t)_ 


com 





Since the path is composed of straight segments and a quarter circle, some of the global 
motion command vectors are discontinuous at the beginning and end of the turn as shown 
in Figures 2.6, 2.7, and 2.8. A more sophisticated trajectory generation method could be 
used to remove these discontinuities. 

Two control cases are performed for tracking the desired path, the first, (Case 1), 
allows all control inputs to be available, while the second, (Case 2), uses no lateral thruster 
assistance, relying only on the rudders and rear screws for control. To demonstrate 
controller robustness, simulation results for Case 1 also contain the responses for varying 
degrees of structured parametric uncertainty. Results using four different levels of 
mismatch in the input gain matrix, g' , and a single value of mismatch for the non-linear 
feedforward terms, /’, are given. The differences from the values used in the model were 



and 



g, = g* 

g, = 10.0.*g' 

gl = /2.a*g‘ 
g* = 0.4.*g‘ 



f, = 



^ 4 . 0 ^ 

4.0 

1.0 
1.0 
4.0 

\1.0 



•*/• 
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No parameter uncertainties for Case 2 were considered nor any disturbances such as 
current, and the tunnel thrusters were modeled as simple forces; no thruster dynamics were 
used. Matrix was chosen for convenience to be identity and since no current was 
present, no integral control was necessary, therefore Sj was set to 0. For matrix S^, a 
diagonal matrix was used of the form 



0 0 0 0 O' 

0 Xy 0 0 0 0 

0 0 0 0 0 

0 0 0 Xg 0 O' 

0 0 0 0 X^ 0 

0 0 0 0 0 X^ 



and with this choice, a simple sliding surface design followed such that the prescribed 
tracking error dynamics on the surface become 

Zi(t) + A,i;(r) = 0 ■, i = X, Y, Z, G, i / a , and (f), 

which has well behaved dynamics, and 

<7(z(0) = (<7x <yy (Jz 

The values of each rj^ and 0,, and A, used in the simulation were: 



Table 2.1 Sliding Mode Control Gains 



Mode 


X 


Y 


Z 


6 


W 




A 


0.5 


0.5 


0.5 


0.5 


1.0 


N/A 


n 


0.5 


0.5 


0.1 


0.1 


0.3 


N/A 


(t> 


1.0 


1.0 


1.0 


1.0 


0.1 


N/A 



Note that the last sliding surface is included, although the roll motion is not controlled. It 
follows that, maintaining dimensional consistency, the controls are now computed as 
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(2.37) 



(2.38) 



Uj(t) = [G* : 0]M’h"'F(a\f) 



(2.39) 



Results of the first case are shown in Figures 2.9 through 2.18. Inspection of the 
position and heading response show that the vehicle perfectly tracked the commanded 
trajectory when using the exact input gain matrix g] in the controller design. Although the 

system is non-minimum phase, the controller design has been able to compensate for this 
since full state feedback is available. Degradation of the tracking response from increasing 
levels of mismatch in g' and /* is evident, however, the trajectories of each (T- approach 

zero as shown in Figures 2.1 1 through 2.13. 

Referring to Figures 2.14 through 2.16, spikes in the control input at the beginning 
and end of the turn are evident, and are caused by the discontinuities of the acceleration 
commands at these locations. Using the input gain matrix g^ in the control design provided 
tracking performance equivalent to using g‘, (no mismatch), but this came at the expense 

of high actuator activity. Figure 2.17 shows a comparison of the stern rudder response for 
both g', and g^. Figure 2.18, shows that the roll response is passively stable and is only 

slightly excited by the turning maneuver. 

The second case uses no lateral thrusters for control. Removal of the thruster 
contribution was accomplished by setting the values and to zero in the weighting 

matrix, W. Figures 2.19 through 2.23 show the results of this simulation, using two 
different weightings for the rudders and Using Weighting 1, = 1.0, 

resulted in tracking errors as shown in Figures 2.19 and 2.20, and was caused by rudder 
saturation (Figure 2.21). The stern rudder, after an initial negative deflection, rotated 
positive in effort to satisfy the lateral force requirement, resulting in a decreased turning 
moment. 

Since rudder saturation is an undesirable condition, a method must be found that 
can reduce the rudder stroke and still maintain accurate tracking. There are two solutions to 
this problem; (1) use larger area rudders to increase their effectiveness, or (2) reduce the 
weights Wg^g and until the maximum stroke reduces below the saturation limit. Using 

the latter will not only reduce control surface saturation, it also prevents the stern rudder 



32 



from decreasing the turning moment. The second set of curves denoted by Weighting 2, 
^srh ~ ^Srs ~ shows that reducing the weights provides perfect tracking and an overall 

reduction in the control effort from both the rudders and rear screws. The roll response is 
again passively stable shown in Figure 2.23. 

1 . Remark on Non-Minimum Phase Effects 

In course of this work, it has become apparent that non-minimum phase effects can 
arise in two different ways. First, combinations of elements in the input dynamics matrices 
(for a linearized system) that result in unstable zeros in some input/output transfer 
functions. Secondly, in some systems, unstable zeros arise because of particular elements 
in the output matrix. Output linearization of a system belonging to the second class will 
exhibit unstable internal dynamics, and can not be stabilized by output feedback. In the first 
case, sliding mode formulations including the complete state, compensate for any unstable 
zeros and are stabilizable. For the case of underwater vehicles with full state feedback, non- 
minimum phase effects on the stem planes and rudders is fully compensated. 

2 . Discrete Time Implementation 

While theoretical development has been performed in the continuous time domain, 
real time control is implemented in the discrete time domain. Time discretization is 
performed using the Euler transformation, x{kT) = - x,.)/T. 

F. CONCLUSIONS 

Results from this chapter have shown that the formulation for a MIMO Sliding 
Mode controller performs very well, even with parameter mismatch. The simulations have 
also shown that adequate control authority is needed when performing path tracking 
maneuvers, and the weights for the inverse solution of the input gain matrix can play an 
important role in the control performance. Further work is needed to automate the 
computation of the weights for the control surfaces and thrusters, preferably as a 
continuous function of vehicle forward speed. From this, control saturation can be kept to 
minimum throughout a given maneuver. 
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A robustness analysis was also presented which included any modeling 
inaccuracies in the mass, dynamics, and input gain parameters. The analysis provided a 
design procedure to ensure stability despite the uncertainties. Further work is needed to 
develop a design methodology to eliminate possible actuator saturation due to these 
modeling errors. 

Although, the control design methodology presented appears robust and well 
behaved in simulation, other real factors not included are lags in thruster response. If 
relatively long lags are present, these effects should be included in the general analysis. 
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2 . 1 Coordinate Frame Representation. 
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.2 Three Possible Switching Functions for the Sliding Mode Controller. 
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2.3 Control Surface Convention of the NPS Phoenix. 





2.4 Thru.ster Force Convention of the NFS Phoenix. 
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2.5 Commanded Trajectory for Vehicle Performance Simulation. 
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.6 Commanded Global Position Profiles vs. Time for Vehicle Performance Simulation. 
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.7 Commanded Global Velocity Profiles vs. Time for Vehicle Performance Simulation. 




2.8 Commanded Global Acceleration Profiles vs. Time for Vehicle Perfonnance 
Simulation. 
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Y(t), (Feet) 



2.9 Position Response for Case 1, Showing the Tracking Degradation Using 
Various Levels of Parameter Mismatch. 
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2.10 Heading Angle Response vs. Time for Case 1, Showing the Tracking Degradation 
Using Various Levels of Parameter Mismatch. 
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2.1 1 Response of G^ Using Various Levels of Parameter Mismatch. 
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2. 12 Response of <Jy Using Various Levels of Parameter Mismatch. 
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2. 1 3 Respon.se of <7^ Using Various Levels of Parameter Mismatch. 
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2. 14 Rudder Deflection Angle vs. Time for Case 1 , Using g] and gi 
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2.15 Lateral Thruster Control Force vs. Time for Case 1, Using gj and g,*. 



51 




2.16 Rear Screw Control Force v.s. Time for Case 1, Using g* and 
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2.17 Rudder Deflection Angle vs. Time for Case 1, Using f, and g]. 
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2.18 Roll Angle vs. Time for Case 1, Using gj and g". 
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2.19 Position Response for Case 2, Weighting 1: (all vv, = 7.0, except 
viv = vvr = 0.0), and Weiahtinq 2; (all u- = 1.0, except uv = u> = 0.0, 

and wg^^ = \vg^^ = 0.n. 
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2.20 Heading Response vs. Time for Case 2, Weighting!: (all w, = 7.0, except 
= 0.0), and Weighting 2: (all vi'^ = 7.0, except = 0.0, 

and =0.7). 
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2.21 Rudder Deflection vs. Time for Case 2, Weighting 1; (all vv, = /.O, except 
= uy _ = 0.0), and Weighting 2: (all w, = 1.0, except vty^^^ = uy^^_ =0.0, 

and = =0.7). 
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2.22 Rear Screw Control Force vs. Time for Case 2, Weighting 1; (all u’, = I.O, except 
= 0.0), and Weighting 2: (all vv, = 1.0, except = 0.0, and 
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2.23 Roll Angle Response vs. Time for Case 2, Weighting 1; (all vv, = 1 . 0 , except 
= 0 . 0 ), and Weighting 2: (all vv,. = 1 . 0 , except = 0 . 0 , and 
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III. ROBOT CONTROL AND THE NFS PHOENIX VEHICLE 



In the previous chapter, a general theory for continuous control of vehicle motion 
was presented. This alone is not sufficient to enable an autonomous vehicle to execute a 
complex mission involving multiple phases requiring different control modes. Vehicle 
motion is continuous, while the sequencing and coordination of different control modes is 
represented by discrete events. 

The development of autonomous underwater vehicle control technology for 
underwater robots lies at the intersection of Discrete Event Systems (DES) and Dynamic 
Control of Continuous Systems (DCS) where system theory is well developed for each 
alone but not both acting together. It is not well understood how to formally evaluate the 
performance of these combined systems that are now being referred to as "Hybrid" control 
systems (Antsaklis and Passino, 1993). Saridis (Saridas, 1989) introduced the concept of 
"entropy" for a multidimensional performance index that could possibly be optimized for 
hybrid systems. Computer Aided Design of these systems has been proposed (Simon, et. 
al., 1993) using a rigid robot manipulator as an example, overcoming the lack of formal 
methods by using ORCCAD, a CAD package and the synchronous language "Esterel" 
developed especially for handling DES as automata. 

Software architectures for underwater vehicles - a distinctly different problem from 
robotic manipulators - involve vehicle stabilization issues, and have been described and 
discussed in previous literature (Hall and Adams, 1992, Albus, 1988, Sousa, et. al., 
1994), but without any experimental validation. Few detailed results have been quantified 
for the Odyssey class of vehicle (Smith and Dunn, 1994, Bellingham, et. al., 1994) 
although the Odyssey has performed under ice and demonstrated homing behaviors into a 
capture net. 

Some Hybrid systems are predominantly DES and can be designed using state 
tables and finite state machines, or recently, Petri net methodologies (Cassandras, 1993). 
Others are predominantly continuous DCS with only a small component of discrete state 
logic for which stability theory, established optimal control techniques, and sliding modes 
are well suited (Friedland, 1986). "Hybrid", in the context of this dissertation, deals with 
the underwater robot control problem which is a true mix of DES and DCS for which new 
design techniques and evaluation methods are needed. In order to separate the functionality 
of the system we note that the control of the sequencing of a mission is a discrete event 
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system (DES) problem with the state transitions driven by conditions arising from the 
completion of robot tasks or by sensor based events, while the stabilization and control of 
vehicle motion to mission derived trajectories and or set points, is a traditional problem in 
dynamic control. 

There is currently a very strong interest among researchers in the fields of artificial 
intelligence and robotics in finding a more effective means of linking high level symbolic 
computations relating to mission planning and control for autonomous vehicles to low level 
vehicle control software. Such research typically results in a proposal for a general 
software architecture, intended to solve a wide class of such problems. One of the first 
such proposals due to Saridis, who defined intelligent control as a research area lying in the 
intersection of artificial intelligence, control theory, and operations research (Saridis, 
1983). He then further explicitly recognized three "basic levels" of control called the 
organizational, coordination, and hardware control levels, respectively. A more in depth 
discussion of this problem is given in (Healey, et. al., 1993). 

The NASREM architecture (Albus, 1990) is at one end of the spectrum of Hybrid 
controllers and relies on a hierarchical system of planning. At the other end is the layered 
control with subsumption (Brooks, 1986) modified with discrete state coordination as in 
(Bellingham and Consi, 1991). The state transitions arise from completion of robot tasks 
while the specifications of a mission phase generates plans for vehicle motions in terms of 
either set points and control mode activations. It is the later that forms the basis for linking 
the mission control (DES) at the top (Strategic Level) to the vehicle control (DCS) at the 
bottom, (Execution Level) and is embodied in a middle (Tactical Level) set of control 
software functions. 

This defines a tri-level software control architecture (- the Rational Behavior Model 
- (Byrnes, 1992, Byrnes, et. al., 1993(b))) comprising Strategic, Tactical, and Execution 
Levels. The three levels separate the software into easily modularized functions 
encompassing everything from logically intense discrete state transitioning through the 
interfacing of asynchronous data updates with the real time synchronized controller 
functions that stabilize the vehicle motion to set points or trajectory commands. 
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The distinguishing features that identify each level are 



1. Strategic Level: 

This level in the architecture uses a rule based language and is entirely Boolean - 
dealing only with the management of the discrete state transitioning required to perform the 
mission control. No numerical computations are done at this level and no memory is 
required except for the phase of the mission. In principle, it determines what needs to be 
done next. 

2. Execution Level: 

This level contains all the code functions that are required to stabilize the motion control 
of the vehicle to a set of commands that could be modes to be activated and servo set points 
where the servo control functions can be complex, even including command overrides for 
reflexive behavior and adaptive control features. Many robot controllers operate at this level 
only. 

3. Tactical Level: 

This level is a set of functions that are compiled as predicates in the Strategic Level 
Rules which open and close lines of communications between the Strategic Level and the 
Execution Level functions. They include the functions that gather data from the servo level 
and perform the necessary computations to determine if the robot tasks are completed, 
perform the navigational planning and replanning functions, the sonar computations, state 
of health diagnostic functions, and evaluates and sends appropriate set points and servo 
mode activation flags to the Execution Level. In this level, the computations are numerical 
but asynchronous with respect to time. The distinguishing feature between the Tactical and 
Execution Level software is that of the need for hard real time completion in the Execution 
Level and asynchronous completion in the Tactical Level. 

In the controller architecture, developed in this work, the Strategic Level uses 
Prolog as a rule based mission control specification language. Other DES control system 
design techniques and implementation methods could be used, although, through the work 
of this dissertation so far it has been found that none is more convenient than using this 
existing available language. Prolog has the advantage of being an executable specification 
language which can run in real time as is demonstrated herein. The DES represented by the 



63 



mission specification could have been be translated in to C, Ada, C++, or other languages 
such as Esterel (Simon, et. al., 1993) and Coral (Silva, et. al., 1994). However, in our use 
of Prolog, the Prolog inference engine cycles through the predicate rules and in doing so, 
manages the state transition aspects of mission control so the need for formal, logical 
verification of the control specification disappears. It transitions the states in real time, and 
generally develops the commands (activations) that drive the vehicle through its mission. 
Error recovery procedures from failures in the mission tasks or the vehicle subsystems are 
handled as transitions to "error" states that ultimately provide commands to the servo level 
control for appropriate recovery action. 

The Tactical Level, currently written in C is set of functions that are linked at 
compile time with the Prolog predicates and are designed to either return TRUE / FALSE in 
response to queries - these are distinguished by the prefix "Ask" in the Prolog rules - or to 
activate commands, distinguished by the prefix "Exec". These Tactical Level functions are 
also interfaced to the real time Execution Level controller using asynchronous 
communications and script type messages passed through an ethernet socket with TCP/IP 
protocol. Mission planning using this system provides a "complete plan". Success is 
guaranteed for every mission phase, either by proper completion, or by an abort. 

The Execution Level controller is designed to command the vehicle subsystems 
appropriately for the mode flags and set points sent on the socket and to activate robot 
behaviors that correspond to those commanded. Communication from the Tactical Level to 
the Execution Level takes place through a single socket. By the design of this hierarchical 
control system, the Tactical Level runs asynchronously and retains the mission data file and 
the mission log file in global memory. It sends the command scripts to the Execution Level 
and requests data for the evaluation of state transitions. The architecture is a hybrid between 
the true hierarchical control of NASREM (Albus, 1990) and purely reactive of subsumption 
(Brooks, 1986) schemes. In this way, control of mission is retained, while reacting to 
unanticipated events is also enabled. 

While earlier results at NPS were obtained by workstation simulations (Byrnes, 
1993(a)), the major contribution of this section of the dissertation is that the tri-level 
software architecture has been implemented and experimentally verified with real time 
hybrid control tests using the NPS Phoenix vehicle. The experiments used all three levels 
of the control software active in real time . In what follows, the details of the vehicle 
hardware, as well as the controller hardware and software configurations are provided with 
discussion of the experimental evaluation of the controller through example. The example is 
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with respect to a simple mission using the hovering mode capabilities of the vehicle 
including coordination of the activation of a high frequency profiling sonar as part of the 
mission plan. 

A. THE NFS PHOENIX AUTONOMOUS UNDERWATER VEHICLE 

The Phoenix is the third generation of Autonomous Underwater Vehicles at the 
Naval Postgraduate School. The first was a two foot long craft which had twin screws and 
rudder was used for parameter identification and control research. There was no onboard 
computer or batteries but was attached to a tether which supplied power and control 
commands from an external PC (Healey, et. al., 1989, Brunner, 1988). This is now 
referred to as the AUV I and is no longer in operation at this time. 

In order to experiment with autonomous control a new vehicle was built named the 
AUV II (Healey and Good, 1992) and was much larger, 6.5 feet in length, weighing 435 
lbs and carried it's own batteries and computer. Many untethered tests of this vehicle were 
carried out in the NPS swimming pool during 1990 and 1991 (Healey, et. al., 1991), in 
which waypoint control, steering, diving, and speed control experiments were performed 
and control laws verified. A single process in the onboard computer controlled all 
maneuvers and was written in the C programming language. All data collected from the 
onboard sensors after a run were uploaded using a serial link connected through the access 
hatch. 

In February 1993, the vehicle interior was accidentally flooded and an extensive 
rebuild was undertaken. The vehicle was again operational by October of that year and 
renamed the "Phoenix". Although Phoenix is the same size and weight, and possesses the 
same propulsors as the AUV II, it is a far superior system in the area of computational 
power and data communications. The internal components of the Phoenix are shown in 
Figures 3.1, 3.2 and external views are shown in Figure 3.3. 

The new vehicle contains two computer systems, one the original Gespac with the 
OS9 real-time operating system and the second a Sun Microsystems Voyager SPARC 
station using the Solaris (Unix) operating system. Both computers communicate using 
thinwire ethemet which also extends outside of the vehicle to workstations on the shore 
either through a hard wire connection or using a radio ethernet link. The radio link can be 
either configured to have the antenna mounted to a float for uninterrupted communications 
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while submerged or mounted directly to the hull for better streamlining but only able to 
upload/down load information while surfaced. 

Most of the experimentation done for this dissertation was with a Sun 
Microsystems Britelite portable SPARC workstation, in place of the Sun Voyager, 
operating outside the hull. It was connected to the vehicle with a thinwire ethemet cable 
using a removable water-proof connector. 

1 . Propulsion Systems and Sensors 

Six propellers are used for maneuvering the vehicle: two open screws located at the 
stern, two vertical and two lateral cross-body thrusters. All are powered from 24 Vdc 
electric motors, computer controlled using pulse width modulated servo amplifiers. Located 
at the back of each motor is an optical encoder which is used to measure angular speed The 
encoder generates a square wave pulse which is counted by the controlling computer. 

a. Stern Screws 

Each stern screw is a brass four blade propeller measuring 4.25 inches in 
diameter. These are connected directly to the motor shafts with no reduction and are each 
capable of delivering up to 5 pounds of bollard pull force (Saunders, 1990, Cody, 1992). 
Both may be independently controlled and are able to spin in either direction. 

b . CrosS’Body Thrusters 

The cross-body thrusters consist of a 3 in. ID aluminum tube with a 
centrally located 4 blade brass propeller shown in Figure 3.4. A spur gear is mounted 
around a 3 inch diameter propeller and driven by a pinion connected to a 24 Vdc motor 
giving a 2.5:1 gear reduction. The only seal is located around the motor shaft which leaves 
the propeller, ring gear, and pinion "wet". The propeller shaft is supported by three equally 
spaced struts at each end. These devices were designed and constructed at NFS since there 
were no commercially available cross-body thrusters this small. The twist of the propeller 
blade is symmetric enabling bi-directional operation delivering approximately 1 .0 pound of 
bollard pull force in either direction (Healey, et. al., 1995). 

All propulsors are powered by 24 Vdc Pitman PITMO DC Model 14202 series 
electric motors. Each is equipped with Hewlett-Packard Model HEDS-5000 series optical 
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encoders attached to the shaft at the back of the motor to measure angular rate. The 
encoders use a 500 aperture code wheel which is between an LED emitter and integrated 
circuit detector. The detector converts the signals to a square wave output which is 
measured by the computer. 

c. Control Surfaces 

Eight control surfaces are present on the vehicle, four rudders and four 
vertical control planes. Each surface is powered by servo motors used by radio controlled 
model aircraft (Airtronix Model 94501). Since the surfaces are independently powered, the 
rudders are electronically coupled so that when a command to turn is given, the upper and 
lower planes will rotate together. The stern rudders will rotate one direction, and the bow 
rudders in the opposite direction which provides higher turning performance. This 
approach is also employed with the vertical control planes as shown in Figure 3.5. 

d. Sensors 

A full description of all vehicle sensors are given in Appendix D. It covers 
the gyroscopes, speed sensor, short baseline navigation system, and GPS components. An 
expanded discussion of the Tritech ST725 and ST 1000 design and operation is given in 
Chapter V. 

2 . Computer System 

A Gespac 68030 microprocessor is used for running the vehicle hardware control 
software. It uses the OS9 operating system which enables real time execution of the 
software. Along with the processor are eleven other Gespac boards interfaced with the 
sensors, motors, control surfaces, networks, etc. A brief description of each board and it's 
primary interfacing function is given in Appendix D. 

The Phoenix has been designed to operate for at least 3 hours on a single battery 
charge. The following section describes The major components of the electrical power 
system are described in Appendix D. 
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3 . Hull Integrity 



The vehicle has four access plates located on top of the hull, which are held in place 
by machine screws and are sealed against leakage using a marine grade putty. To ensure 
against leakage through the plate seams, or any other point, the interior is pressurized with 
air to approximately 0.5 lbs above atmospheric. This procedure serves two purposes, one 
is that if the hull is not leak-proof, the pressurized air will escape through the openings and 
can be detected by applying soapy water around the seams to check for bubbles. The 
second reason for pressurization is that if a small opening develops while submerged, the 
air will seep out before water enters the hull. The escaping air will form bubbles that will 
provide a visual queue that a leak is immanent once the internal air pressure drops below 
the hydrostatic pressure. Detection of the bubbles allows for corrective action to be taken. 

B . THE CONTROL NETWORK EXPERIMENTAL SETUP 

The control system, illustrated in its simplest form in Figure 3.6, is currently 
implemented in hardware using three networked processors. All Execution Level software 
is written in "C" and runs on a Gespac M68030 processor in a separate card cage inside the 
boat. Connected in the same card cage is an ethernet card and an array of real time 
interfacing devices for communications to sensors and actuators indicated in the details of 
Figure 3.7. The Execution Level control code containing a set of functions in a compiled 
module called "exec” is downloaded first, opening the communications socket on the 
Gespac side. This process block sleeps until a network connection is requested from the 
higher level controller in the Sun SPARC. 

The network configuration for the results presented is shown in Figure 3.8. It 
consists of five nodes connected by thinwire ethernet. The Gespac computer inside the 
vehicle is connected to the other systems using a water-proof through-hull connector. The 
DOS PC is used only for OS9 cross-compilation of C code usually developed on an SGI 
Elan or equivalent system. The Elan is also used for displaying in real time the sonar data 
obtained in the Execution Level for missions using the STIOOO sonar. To further facilitate 
software development and transfer, a wireless ethernet unit is available for Internet 
connections outside of the laboratory. 
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C. THE TRI-LEVEL CONTROL SYSTEM 
1 . Vehicle Primitives 

In previous work (Healey and Marco, 1992(a)), waypoint following in a transit 
phase of a mission was demonstrated in a swimming pool test area where stable behaviors 
of the vehicle were demonstrated including 

a) Forward Speed Control, 

b) Fin_Steering 

c) Fin_Depth_Control 

d) Waypoint_Following 

e) Bottom_Following, and 

f) Obstacle_Avoidance. 

These control functions were implemented with a)-c) and f) running simultaneously, but 
subsumed by the guidance laws implemented in d); and, with c) subsumed by e). The 
control laws implemented have been based on PD, and Sliding Mode methods as explained 
in (Healey and Marco, 1992(b)). 

Control laws for these functions are readily implemented entirely in the Execution 
Level with digital control algorithms running at 0.1 sec. update rate. Now, more complex 
functions have been enabled using active control of the cross-body thrusters and sonar. 
These are, 

g) Submerge_and_Pitch_Control 

h) Heading_Control 

i) Longitudinal_Positional_Control 

j) Speed_Control using command generators 

k) Lateral_Positional_Control 

l) Center_Sonar 

m) Ping Sonar (Mode 0) 

n) Ping and Rotate Sonar Clockwise (Mode -f 1) 

o) Ping and Rotate Sonar Counter-Clockwise (Mode - 1 ) 

p) Ping and Rotate Sonar Through a Sector (Mode 2) 
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q) Initiate_Filter_For_Sonar_Range 

(Needed For Smoothed Range and Range Rate Estimation) 

r) Reinitialize_Filter 

s) Track_Target 

t) XY_Psi_Control 

Most of these functions need a given subset of the actuator system to be active 
under the operation of either an open loop command or a feedback control law. Some of the 
functions use orthogonal sets of actuators and may be activated without conflict. Some use 
the same actuators to control different functions and thus control laws may be additive. 
This means, for example, that vertical thrusters may be used via control laws to control 
depth as well as pitch, and lateral thrusters to control heading as well as lateral position and 
side slip speed. In combination with propulsion motors, most functions including 
Submerge_and_Pitch_Control, Longitudinal_Speed_Control and 
Longitudinal_Position_Control, as well as Heading_Control, may now be commanded 
reliably. Heading_Control, Submerge_and_Pitch_Control, and virtually any multiple 
combination of a) to t) above are available to the extent that they do not cause a conflict of 
actuator control or sensor usage. 

2 . Orthogonal Behaviors 

Orthogonal behaviors are defined as those control behaviors that use non-interacting 
subsets of actuators. Even though each may use some common sensory data, orthogonal 
behavior control functions may be activated simultaneously without conflict. An example 
would be Heading_Control (using lateral thrusters), Longitudinal_Position_Control (using 
the propulsion motors) and Center_Sonar. Non-orthogonal behaviors use intersecting sets 
of actuators for control of different error functions and thus control laws can be built up 
from linear combinations of individual control laws - as used for combined heave and pitch 
control using vertical thrusters. 

Activation of orthogonal behaviors are instituted using a script composed of flags 
and set points that are a way of communicating between Tactical Level C functions and the 
real time control loop of the Execution Level. At each pass through the loop, a read is made 
from the communications socket and an if-else structure is used to determine which set of 
sensors, actuators and control laws are to be activated during the computation cycle. The 
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same technique is used to flag the activation of sonars, and filtering actions, and similarly 
for flags to indicate which data streams are to be written in return. 

3 . Reactive Behavior Implementation 

Reactive behavior in the controller is handled in three ways similarly to that done in 
the ORCCAD design (Simon, et. al., 1993). 

1. In the Execution Level control loop through command overrides 
following a sensor read, as, for instance, a new obstacle detection requiring 
an emergency surface or obstacle avoidance (flinch) response, 

2 . at the Tactical Level, reactive error recovery can be handled by resetting 
key parameters associated with control signal evaluations. An example is the 
resetting of a control gain or the inclusion of integral control if a particular 
error function cannot be stabilized, 

3 . reactive behavior is also handled at the Strategic Level for catastrophic 
faults by transitioning to states that command fatal error recovery 
procedures. An example is to surface if, for example, a particular mission 
phase is not completed after a pre-specified time out and all other techniques 
have been exhausted. 

In the results described here, reactive behavior is built in at the Strategic Level by 
time and space traps using time out calls. If an allocated time is exceeded, the mission 
phase fails and the vehicle is commanded to surface. Control overrides are built into the 
Execution Level to surface the vehicle if battery power is too low or if a leak is detected. 

4 . Strategic Level 

The Strategic Level Prolog rules are compiled and linked together with the 
supporting Tactical Level "C" language functions into a single executable process called 
”Mission_Control", that is run in a Sun Microsystems SPARC 4 laptop computer and 
linked through an ethernet socket to the Gespac processor (socket "A" in Figure 3.6). 
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Starting "Mission_Control" enables communications between both SPARC and Gespac 
processes. All vehicle control functions, with the exception of the transmission of sonar 
imaging data, communicate by message passing through that socket. The Strategic Level 
Prolog is divided into two parts - first the generic mission controller in Figure 3.9 (Marco, 
et. al., 1996), and secondly, the phase level detail in Figures 3.10-3.12. For clarity, the 
higher level rules are shown in bold type, the C functions in italics, and any user defined or 
built-in Prolog predicates are in plain text. The rule set is first compiled and then run in the 
interpreter by entering the query "execute_mission.". The example mission outlined in the 
figures below consists of three phases: vehicle initialization; submerging to a specified 
depth while maintaining a heading command; and sweeping the profiling sonar head 360 
degrees while still controlling to depth and heading. This is a deliberately simplified 
mission for clarity of explanation. Many more complex missions have been performed 
utilizing the principles outlined here. 

A complete listing and description of each C function callable by Prolog is in 
Appendix E. 



done current_phase(mission_abort). 
done current_phase{mission_complete). 

execute_mission initialize.mission, repeat, execute_phase, done. 

initialize_mission ood{'start_networks\X), asserta(current_phase(l)), asserta(complete(0)). 
asserta(abort(0)). 

execute_phase current_phase(X), execute_phase(X), next_phase(X),!. 



Figure 3.9 Prolog for the Generic Mission Controller. 
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execute_phase(l) exec _init_vehicle(X), exec _start_timer(X), repeat, 

phase_completed(l). 

phase_completed(l) ask_init_vehicle_done(X}, X==l,asserta(complete(l)). 
phase_completed(l) ask_time_out(X), X==l, asseria(abort(l)). 

next_phase(l) complete(l), retract(current_phase(l)), asserta(currenl_phase(2)). 
next_phase(l) abort(l), retract(currenl_phase(l)), asserta(current_phase(mission_abort)). 



Figure 3.10 Prolog for Phase 1 (Initialize Vehicle Systems). 



execute_phase(2) exec _get_setpoints(X)y exec_submerge(X), exec_rotate(X) , 
exec _start_timer(X), repeat, phase_completed(2). 

phase_compleled(2) ask_depth_reach€d(X), X=l, ask_h€ading_reached{X), X==l, asseria(complete(2)). 

phase_completed(2) ask_jime_out(X), X==l, ex€c_surface(X), repeat, ask^surface^reachediX), X— 1, 
asserta(abort(2)). 

phase_completed(2) ask_sys_problem(X), X=l, exec_surface(X), repeat, ask_surf_reached(X)y X==l, 
asserta(abort(2)). 

next_phase(2) complete(2), retract(current_phase(2)), asserta(current_phase(3)). 
next_phase{2) abort(2), retract(current_phase(2)), asserta(current_phase(mission_abort)). 



Figure 3. 1 1 Prolog for Phase 2 (Submerge to Depth and Rotate to Heading). 
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execute_phase(3) exec_get_setpoints(X), exec _suhmerge(X) , exec _rotate(X) y 

€xec_set_sonar_mode(X)y exec_start_timer(X) , repeat, 
phase_compIeted(3). 

phase_completed(3) ask_sonar_sweep_complete(X), X==l, asserta(complete(3)). 

phase_completed(3) ask_nme_out(X), X==l, exec_surface(X), repeat, ask_surface_reached(X), X=l, 
asseita(abort(3)). 

phase_completed(3) > ask^sys _problem(Xj, X==l, exec_surface(X), repeat, ask_surfacejreached{X}y X==l, 
asserta(abort(3)). 

next_phase(3) complete(3), retract(current_phase(3)), asserta(current_phase(mission_complete)). 
next_phase(3) abort(3), retract(current_phase(3)), asserta(current_phase(mission_abort)). 



Figure 3.12 Prolog for Phase 3 (Sweep Sonar). 

Referring to the Prolog code in Figure 3.9, the rule head "execute_mission" will be 
satisfied, and hence the mission completed if the predicates ''initialize_mission", 
"execute_phase", and "done" all evaluate TRUE. Once "ood('start_networks',X)" 
completes, phase 1 is asserted to be the current phase (i.e. set to TRUE), then the entire 
rule body of "initialize_mission" is evaluated as TRUE. This action enables the control to 
enter a repeat loop which executes the predicate "execute_phase" attempting to evaluate 
each predicate current_phase(X), execute_phase(X), and next_phase(X), as X assumes the 
values 1 through 3 in sequential order. This particular mission has only three phases, but is 
expandable to include as many phases as desired. 

Each phase includes a "repeat" predicate so that the rules for phase completion are 
evaluated repetitively until one of the rules is TRUE. With the exception of vehicle 
initialization, each phase can terminate in one of three ways: 

1 . Normal Completion. Next Action: (Execute Next Phase) 

2. Abort Due to Time Out. Next Action: (Surface Immediately) 

3. Abort Due to System Problem. Next Action: (Surface Immediately) 

If phase X completes normally, "complete(X)" is asserted and X is incremented by one to 
execute the next phase. Normal completion usually indicates that a commanded set point or 
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task has been accomplished and the vehicle is ready to start the next phase. In the case of 
phase 2, this means that the commanded depth and heading has been achieved. If a time out 
or a system problem occurs, the vehicle is commanded to surface immediately and a 
mission abort for phase X is asserted after the surface is reached. A time out indicates that a 
set point or task is taking too much time to complete and with our current version of error 
recovery, the mission phase is aborted and also the entire mission. System problems can 
cover a variety malfunctions, sensor failures, thruster failures, or any type of hardware 
problem, which for this work, are assumed to be catastrophic requiring an entire mission 
abort. 

After each phase executes, the predicate "done" is evaluated. If the next phase is 
commanded, "done" fails and the cycle continues, if however a mission abort is asserted or 
the mission completes, "done" is satisfied and "execute_mission" evaluates TRUE and the 
entire mission is finished. 

Tasks that are required to be performed in successive phases are commanded again 
as shown by the calls exec _submerge(X), exec _rotate(X), which appear in 

Phase 2 and 3 (Figures 3.1 1 and 3. 12 ). In this way new set points can be entered for each 
phase. Generally control mode commands are left active until changed although "kill" rules 
can be used to stop actions such as filters etc. 

5 . Tactical Level 

The Tactical Level of the control system contains all the C functions that are 
compiled as predicates in the Strategic Level rules, and performs the computations upon 
which the vehicle commands and transitions are based. Additionally, a second SPARC 
process called the "Sonar Manager" is opened which runs asynchronously in the 
SPARC and with equal priority to the "Mission_ControI". This process is linked 
through a separate socket ("B" in Figure 3.6) to the Gespac for the purpose of the reception 
and handling of sonar imaging data. The "Sonar Manager" captures data that is sent out 
from the Execution Level as soon as it has been acquired, and then processes and passes 
the data to be displayed on the IRIS Graphics workstation for visualization purposes. 

The introduction of the additional process called "Sonar Manager" and it’s 
separation from the "Mission_ControI" Tactical Level functions has been found to be 
important and a necessary first step toward a more general Concurrent Tactical Level that 
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was foreseen by the earlier RBM architecture (Byrnes, et. al., 1993(b)) and explained 
recently by (Kwak and Thornton, 1994). The need for concurrency of multiple processes 
lies fundamentally with the fact that sonar data is obtained asynchronously with bounded 
but unknown latency and the servo control functions cannot wait for the sonar port data to 
arrive. While it is perfectly normal to send control set point commands asynchronously to 
stable control loops, waiting for sonar returns could hold up the servicing of the inner 
servo loop commands to actuators. Thus in this solution to the Hybrid control problem, the 
additional "Sonar Manager" process always reads the socket onto which sonar data is 
written so that it is immediately free for another sonar write without delay, and the servo 
loop is made independent of direct involvement with the sonar. As an unpleasant 
alternative, this research has shown that without the "Sonar Manager", all the Tactical 
Level functions would have to be modified to include a check to read sonar data. This 
would have been a cumbersome addition of much unnecessary code writing. One possible 
alternative, yet to be fully implemented, is to allow the primary acquisition of sonar to 
communicate directly and asynchronously in the Tactical Level. However, the drawback of 
that approach would be the difficulty of obtaining high rate sonar updates in servo control 
functions. 



a. Transition Criteria 

Most control phase transitions of the Phoenix are event based, meaning that 
a certain set of criteria must be met in order for a transition to occur. A common example of 
this is when a position set point is sent to the vehicle controllers and reached. A method of 
determining whether the vehicle has indeed reached this point must be programmed into the 
control logic. Measuring the position error alone and declaring the maneuver complete 
when this error is small is not sufficient. This is because the vehicle could be overshooting 
the commanded position and simply passing through the set point. Not only must the 
position error be small but the rate error must also be small. This dual criteria can be 
expressed mathematically as a positive definite, linear combination of the position error e 
and the position rate error e. We use, 

(3-1) 
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where and wr are positive weights for the position and rate errors respectively. Eqn. 
(3.1) may be quantized, which allows a minimum value of k, denoted Kg, to be specified 
defining a threshold for the combination of errors which can be set relatively large when 
precision control is not required or low for extremely precise positioning. Once K drops 
below Kg, the maneuver is declared complete and a transition to the next control phase may 

occur. 

When noisy sensors are used, the noise prevents k from settling enough to 
determine an accurate measurement for the transition, and the use of Eqn. (3.1) alone has 
shown to be unsatisfactory. The signal can be smoothed by filtering K through a first 
order digital filter of the form 

+ (I - (3.2) 

where is the filtered form of k, t is the time constant of the filter, and T is the 

sampling time. The condition for transition can be shown in Figure 3.13, which indicates 
that the signal for transition, 5 , is 1 (TRUE) for K^ < Kj^^ or 0 (FALSE) for Kj > Kj^^. As 

an example, the function "ask_depth_reached(X)", performs the calculations above and 
returns s. Other dynamic error and time based signals are computed similarly. 

b . Mission Data File 

Each phase with the exception of initialization has associated with it 
particular mission parameters. These tend to be set points, phase time-outs, etc. This 
information is obtained by the Tactical Level through the use of a mission data file which 
contains numerical values for each phase. The file is named "mission. d" and has the 
following format for each phase. 
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Timeout (Sec) X^„„(Ft) 


>^„™(Ft) 


2.,. (Ft) 


(Rad) 


^c„JRad) 


V'c.CRad) 








K^o.md) 




v^o^(Rad) 


Sweep_Time (Sec) 


Sweep_Mode V',w(Deg) W^,(Dcg) 




CM, 


CMy 


CM, 


CM, 


CM, 


CM^ 


LT, (Sec) 


ATyiStc) 

• 

• 

• 


AT,{Scc) 


AT, (See) 


AT,(Sec) 


AT/Sec) 



At time of initialization the entire file is read and each time the predicate 
exec_get_setpoints(X) is executed, the data for that phase is obtained. For each phase, 

totaling "N_phases", a vector of position and angular set points is defined along with the 
minimum value for the filtered error, Kjg, for each control direction. The values 

Sweep_Mode, (Deg) and (Deg) are initial sonar control parameters which may or 
may not change during the phase depending on the mission specification. The "CM" 
(Control Mode) values are either 0, for step input control or 1 , which indicates a command 
generator should be used. If command generators are used, the time to complete the 
maneuver may be specified using the AT's in the last row (see Appendix B). With this file, 
many combinations of control can be selected. For example, any phase may use a 
command generator for depth control but not for heading, or this control mode may be 
specified for both. Any combination is possible provided the particular degree-of-freedom 
is controllable. For the case of submergence only, with no provision for the positions X 
and Y to be measured or controlled in the mission specification, the values of and 
^com meaningless and any value may be entered. Common practice is to simply enter 
0.0 for the terms not pertinent to a particular phase. 

6 . Execution Level 

The structure of the Execution Level software illustrated by Figure 3.7 indicates that 
it is composed of software at the hardware interface (software drivers) as well as software 
for vehicle control. It is the primary set of software that performs real time data acquisition 
and servo control. After initialization of power systems and sonars, and the basic driver 
settings, the PIA card pins that control the on/off features of power supplies, thruster 
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power, screw power, and sonar power, a simple timing loop is entered and reentered at a 
fixed update rate (in our case 0.1 sec.). During each loop the following takes place and is 
shown in Figure 3.14. 

1 . Read socket "A" for behavior based mode command flags and control set points 
(SOCKET READ BLOCK). 

2. Read all sensors (READ SENSORS BLOCK). 

3. Select appropriate C code control functions for computing and sending control 
values to actuators, using an if-else structure for distinguishing the commands and 
send signals to the sonars to ping and rotate, etc. (CONTROL BLOCK). 

4. Write selected data to local ramdisk/memory or to sockets "A" or ”B" as 
appropriate (DATA RECORDING). 

5. Check time for any time based events and wait for the next timing interrupt to 
maintain integrity of the digital control loop (TIMER WATT). 

Specific control laws as built into callable modules of code are easily selected 
according to the communication flags, provided that they exist in the first place. 

The hardware components of the Phoenix are controlled and interrogated using 
Execution Level C language functions. Each either reads information from the sensors or 
writes commands to the vehicle actuators. The function descriptions are described in detail 
in Appendix F. 

An example of the 3 levels of communication interactions can be seen using 
pseudo-code in Figures 3.15 and 3.16 and from the following code fragments. 



STRATEGIC LEVEL IPROLOG^ 



...exec_get_setpoints(X)... 
...exec_submerge(X), ... 

...repeat, ask_depth_reached(X),... 



TACTICAL LEVEL tCi 



int exec_get_setpoints() 

( 

++cuirent_setpt_index; /* Increment the Set Point Index */ 
retum(TRUE); 

} 



int exec_submerge() 

{ 

sprintf(command_sent,"%s %f %f, "SUB MERGE", z_setpt[cuirent_setpt_index], 

theta_setpt[current_setpt_index]); /* Command vehicle to Submerge to z_setpt */ 
write_to_execution(command_sent); 
retum(TRUE); 

} 



int ask_depth_reached() 

( 

sprintf(command_sent,"%s","GET_DEPTH_INFO"); 

write_to_execution(command_sent); /* Request Depth Information from Execution Level */ 
read_from_execution(&command_read[0]); /* Read Reply from Execution Level , Blocking 

Socket */ 

sscanf(command_read,"%F %F",&z_est,&kappa_zf); 

if( kappa_zf < kappa_zf_min[current_setptjndex] ) 

{ 

return(TRUE); /* Within Minimum Error */ 



else 

{ 

return(FALSE); /* Outside Minimum Error */ 

} 



EXECUTION LEVEL 



{ 

/* INITIALIZATION BLOCK CODE */ 

) 



/* CONTROL LOOP */ 
while (shutdown_signal_received = FALSE) 
{ 

I* SOCKET READ BLOCK */ 
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/* Read Command (If Any) From Tactical Level */ 

read_status = read_from_tactical(&command_read[0]); /* Non-Blocking Socket Read */ 
if(read_status > 0) 

{ 

sscanf(command_read,"%s",&command[0]); /* Extract the Command Only! */ 

/* Switch to the Appropriate Command Parser */ 
if(!strcmp(command/'SUBMERGE")) 

{ 

sscanf(command_read,"%s %F %F %F %F %d %d", 

&command[0],&z_com,&T_z_f,&theta_com,&T_theta_f, 

&submerge_mode,&pitch_mode); 

DEPTH_AND_PITCH_CONTROL = TRUE; 

} 

else if(!strcmp(command,"GET_DEPTH_INFO")) 

{ 

sprintf(command_sent,"%f %f',z_est,kappa_zO; 
write_to_sun(command_sent); 

} 



/* CONTROL BLOCK */ 

if(DEPTH_AND_PITCH_CONTROL) 

{ 

/* Use Com_gen Otherwise use z_com as a step input */ 
if(submerge_mode = 1 ) 

{ 

/* Command Generator */ 

com_gen_z(zO,z_f,T_z_f,&z_com,&zdot_com,&zddot_com,&z_cg_init); 

) 

if(pitch_mode == 1) 

{ 

/* Command Generator */ 

com_gen_theta(thetaO,theta_f,T_theta_f,&theta_com,&thetadot_com, 

&thetaddot_com,&theta_cg_init): 

) 



depth_and_pitch_control(z_com,zdot_com,zddot_com,submerge_mode, 

theta_com,thetadot_com,thetaddot_com,pitch_mode); 



} 



This shows how the Strategic Level communicates with the Tactical Level which in 
turn sends command strings to the Execution Level for submerging control. When the 
Prolog predicate "exec_submerge(X)" is executed, the "C" function in the Tactical Level is 
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called, and writes the command SUBMERGE along with the depth and pitch angle set 
point for the particular phase to the Execution Level. This function then completes and 
having sent the command to the Execution Level, returns a value of TRUE. At this time the 
Execution Level extracts which command has been sent and program control is switched to 
the appropriate command parser block. Since SUBMERGE was sent, the command parser 
expects two set points, depth and pitch angle. Once this command has been received, a flag 
DEPTH_AND_PITCH_CONTROL is set TRUE which activates the associated function in 
the control block and will remain in effect until commanded otherwise. 

7 . Socket Communications (Tactical / Execution Level) 

Careful attention must be paid to both sides of a communications socket when 
dealing with synchronous and asynchronous processes. Reading from a "blocking" socket 
causes execution to pause until data is received. In contrast to that, a "non-blocking" socket 
allows execution to proceed if no data is waiting to be read. For synchronous real time 
execution of dynamic processes attempting to make a read every time step, a "non- 
blocking" socket is a requirement. Since the Tactical Level sends commands and receives 
data asynchronously, while the Execution Level must run synchronously at 10 Hz., the 
UNIX side of socket A is configured to be "blocking", while the OS-9 side is "non- 
blocking". For instance, eight different types of socket communications are used by the 
Esterel language mentioned previously (Simon, et. al., 1993). 

Execution of the predicate "ask_depth_reached(X)" sends a request to the Execution 
Level for depth information (GET_DEPTH_ENFO). The command is parsed in exactly the 
same way as before except that the Tactical Level function waits ("blocking" socket) for the 

Execution Level to return the values of depth and filtered depth error. A comparison is then 
made between the current filtered error, , and the pre-specified minimum, . and the 

function returns TRUE or FALSE as appropriate. 

8 . Human Supervision 

Human supervisory control has not been built into the control system to date. This 
does not mean that it is impossible to do. In fact, user inquiry for the state of the vehicle 
can easily be incorporated into a Tactical Level function that reads an acoustic modem and 
waits for messages to be received. The Strategic Level can include predicates that ask if a 
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user message has been received, and the Tactical Level message can be parsed into 
commands that could call any of the vehicle primitives directly - or specifically - request 
data to be changed. While the architecture supports supervisory control, that is not the main 
focus of this dissertation. 



D. RESULTS FROM AN EXPERIMENTAL MISSION 



The mission described in this work is one of many used to verify this design of a 
Hybrid controller, and was performed in the NPS hover tank. During execution all 
pertinent data was collected, including depth, heading, thruster motor speed, etc. During 
phases where the sonar is active, the range and heading angle of the sonar head was 
recorded. A log file of mission status messages, a time history of the depth response of all 
three phases, and a plot of the profiling sonar image of the tank was obtained. 

While the mission executes, the process running the Strategic Level displays status 
messages to the screen, while others are written by the Tactical Level C functions. Stored 
in a log file for each mission, the following was obtained with messages from the Strategic 
Level in upper case and in lower case for the Tactical Level. 



?- execute_mission. 



INITIALIZE MISSION! 
START NETWORK! 
READ MISSION HLE! 
2 



60.0 


0.0 


0.0 


2.5 


0.0 


0.0 


0.0 


0.0 


0.0 


0.1 


0.0 


0.0 


0.1 


0.0 


0 0.0 


0.0 


00 


00 


00 


O 

d 


o 

d 


o 

d 


o 

d 


o 

d 


o 

d 
















60.0 


o 

d 


o 

d 


2.5 


o 

d 


o 

d 


o 

d 


o 

d 


o 

d 


0.1 


0.0 


0.0 


0.1 


60.0 


1 0.0 


0.0 : mission file 


0 0 


0 0 


0 0 


0.0 


0.0 


0.0 


0.0 


0.0 


0.0 

















Mission File opened successfully. 

START PHASE 1 ! 

INITIALIZE VEHICLE! 

INITIALIZE BOARDS! 

TURN ON PROP POWER! 

TURN ON SONAR POWER! 

UNCAGE DIRECTIONAL GYROSCOPE! 
DIRECTIONAL GYROSCOPE UNCAGED. 
ZEROING SENSORS. 

INITIALIZE STIOOO SONAR! 
INITIALIZATION DONE. 

PHASE 1 COMPLETE. 

START PHASE 2! 
current_setpt_index = 0 
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SLIBMERGE! 

z_setpt = 2.5 theta_setpt = 0.0 

ROTATE! 
psi_setpt = 0.0 

START_TIMER! 

DEPTH TRANSITION. 

Depth @ transition = 2.401626 
z_dot @ switch = 0.004895 
kappa_zf @ transition = 0.093 

HEADING TRANSITION. 
Heading @ switch = -0.033422 
r @ switch = 0.003719 
kappa_psif @ transition = 0.084 
PHASE 2 COMPLETE. 

START PHASE 3! 

START SWEEP TIMER! 

SET SONAR MODE! 

START TIMER! 

SONAR SWEEP COMPLETE. 
PHASE 3 COMPLETE. 

DIS-CONNECT NETWORKS! 
MISSION COMPLETE. 



yes 

I?- 

The commanded depth was 2.5 feet with a filtered error threshold 0. 1 feet. The set 
points for both pitch and heading angle were 0.0 radians, and the sonar was set to 
continuously sweep clockwise (Mode -t-1) for 60.0 seconds in phase 3. After the network 
connections to the various processes were established, the mission file was read by the 
Tactical Level. Although this was a three phase mission, only two rows of set points were 
required, since vehicle initialization does not require set point data. 

The first column of the mission data file is the time out for a phase (seconds), the 

next six columns are the set points for longitudinal, lateral, depth, roll, pitch, and heading. 
The second set of six columns are their respective filtered error thresholds, , and the last 

four columns contain the duration of the sonar sweep, the sweep mode, scan direction, and 
sweep width. A log file is used to show the status of the various transitions and numerical 
values for certain variables of interest. Upon completion of phase 3, the network 
connections are terminated and the mission completes. 
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Figure 3.17 shows the time history of the depth and depth rate response. The lower 
trace shows the behavior of the filtered depth error, , and the threshold for the filtered 

error, . The time axis includes a short time for initialization, and in phase 2 it can be 
seen that starts to reduce as the vehicle begins to submerge at T,. The transition to 
phase 3 is triggered as reaches ( T,), when the sonar is activated and an image of 

the test tank walls and a cylindrical object is recorded as shown by Figure 3.18. While this 
phase is active, the depth controller continues to operate and reduces the error beyond the 
threshold of 0. 1 feet to nearly zero. Once the sonar sweep time is over, the mission 
terminates at time Ty 

E. ARCHITECTURE EVALUATION 

It is not an easy task to evaluate a given control system architecture. The theoretical 
design for stability and robustness leads to selection of parameters that are used in the 
control functions of the Execution Level. While stability and robustness of the mission 
control is not easily proven to the same degree as Execution Level functions, the design 
presented here provides the following positive features. 

1) Evaluation of the controller response is provided from the mission data files, 
and all control parameters are in variable form which can be easily changed to tune 
the low level servos. 

2) In this design of the mission control, every phase can be transitioned, by either 
normal completion, a time-out, or by abort. Therefore, no deadlocks are possible 
since the three termination states are all reachable. 

3) When new sensors or actuators are added to the vehicle, the associated data 
input/output can be integrated into the control software by simply creating new 
functions in the Tactical and Execution Levels. Inclusion of the new sensor/actuator 
will not affect the functionality of the existing code, and the input/output 
corresponding the device is obtained by a single point modification of the data 
recording function. 
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4) Only the Strategic Level code requires modification if the mission is altered to 
eliminate, or add, a new phase. Since the phases are essentially generic in structure, 
and can be described by combinations of existing mission primitives, the code 
changes are usually minor. 

5) It is a simple matter to test the performance of a single sensor or actuator set by 
using the corresponding mission primitive in the Strategic Level, while omitting the 
ones that are not of interest. 

6) Conditions that define the transition signals can be easily adjusted by changing 
the entries in the Tactical Level mission data file. As with the control parameters, 
the transition thresholds are in variable form that can be tailored for particular 
operating conditions. 

F . RELATIONS BETWEEN PROLOG AND PETRI NETS FOR 
STRATEGIC LEVEL IMPLEMENTATION 

So far the mission controller has been represented with Prolog rules. While this is 
the actual language that is used to drive the Strategic Level in real time, there also exists a 
method to graphically model discrete mission events. These models are referred to as Petri 
nets (Murata, 1989), and can sometimes give a more clear and intuitive representation for a 
Strategic Level mission controller. It should be pointed out that using a Petri net graph is 
not intended to replace the Prolog code, but rather provides a different representation of the 
mission controller. The following sections show how Prolog code may be written given a 
Petri net graph, followed by a Petri net analog of the Prolog mission code outlined before. 
It should be noted that our use of Prolog is greatly simplified and so are the Petri net 
representations, being limited to single token places corresponding to the TRUE/FALSE 
evaluations of the predicate rules. 

Three example Petri net graphs and their Prolog representations are shown in 
Figures 3.19-3.21. The circles denoted by P,, Pj, ....,Pn are the states or places of the 
Petri net graph and the bars labeled t, , t^,. . . , are the transitions, where a token enters a 
place whenever a transition is fired. The basic technique for writing Prolog code from an 
existing Petri net is manually done and not optimized, but to start at the terminal state(s) and 
work back towards the initial state of the graph (following the backward chaining nature of 
Prolog). While doing this we define "place" and "transition" predicates, where "place" 
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predicates evaluate TRUE if a token resides there, and a "transition" predicate is TRUE if it 
is enabled and the transition has fired. 

Using this approach, Prolog code has been generated for three example Petri net 
graphs. Each Prolog example is driven by executing "mission_complete" which is the rule 
to be satisfied for completion with the predicate "done" repeatedly queried until TRUE. The 
"place" and "transition" predicates are denoted by p(n) and t(n) respectively. The "ask" 
predicate shown in these examples allows the user to interactively activate the firing of a 
transition by typing a 1 for TRUE or 0 for FALSE. 

The Petri net of Example 1 shows two terminal states, Pj and Pj, which implies 

that for completion, two instances of the Prolog "done" predicate must be defined, (done 
p2. or done p3.). Starting with the terminal states and working back, we must determine 
what conditions must be satisfied to reach these states. The precondition for a transition to 
either Pj or Pj requires a token in place P,. This is assured since the rule for transition t, 
is declared to be TRUE. At this point both transitions tj and tj are "enabled" and either 
may fire to move the token to terminal states P^ or Pj. 

Example 2 is an OR structure with a single terminal state Pj (done p2.) which 
may be reached one of four ways. The first three through transitions tj, tj, or ( 4 , and a 
fourth, directly through tj. This requires the Prolog to have four instances of the rule P^, 

which reflects the OR nature of the Petri net. 

Example 3 is an AND structure with a single terminal place, P 3 (done p3.), 
where both places P, and Pj must be occupied to enable transition tj. This is described in 
Prolog by the AND (pi, p2) in the rule body of p3. 

In the example mission, completing a phase normally, a phase time out, or having a 
system problem are all examples of a discrete event. In a Petri net graph, these are the 
transitions, the places represent the execution of mission commands such as "submerge", 
"rotate", etc. A Prolog "repeat" loop is also considered to be a place with it's transition 
predicates continually evaluated. Figure 3.22 shows the Petri net graph for the mission, 
which starts with a transition at t, (equivalent to querying the rule "execute_mission"). The 
places Pj, Pj and Pj represent the Prolog phases 1, 2 and 3 and are expanded in detail in 
Figures 3.23, 3.24, and 3.25 respectively. The transitions tj, and tj through t,j denoted 
with a thick line are only evaluated if enabled, and fired when the associated predicate 
becomes TRUE, (X==l). A transition drawn with a thin line fires as soon it is enabled. In 
Petri net notation, we are using a "timed" graph since there are definite time delays between 
the enabling and subsequent firing of some - but not all - transitions. Referring to the 
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expanded Petri net graph of phase 2 in Figure 3 . 24 , the transition will fire when the 

rule "execute_phase( 2 )" is executed. Once this has occurred, the predicates represented by 
, P23, and Pj4 are all executed and upon completion, the transition t^2 fires 
immediately, and the place P25 becomes active. At this point the transitions t^, t^, and 
are enabled and the predicates associated with them are evaluated repetitively until one 

of them is TRUE, at which time the respective transition will fire. If a time out or system 
problem does not occur and the desired depth is reached, t2j will fire, and the predicate 

"ask_heading_reached" must also be evaluated repetitively. Since the Prolog repeat 
continues to evaluate transitions, 123 must return a token to P2S causing tj, t^, 123, and 
t24 to remain enabled and evaluated. When the heading is reached, tj4 is fired, 135 is 
enabled, and fires immediately completing phase 2 normally. A similar structure of phase 
completion and error recovery is used in phase 3 as well, which can serve as a template for 
most any mission phase. 

If a time out or system problem occurs in either phase, it is clearly shown that one 
of the transitions through t, will fire and the "exec_surface" predicate (place Pj) will be 

executed. In this state the predicate "ask_surf_reached" is continually queried until the 
surface is reached at which time the mission is aborted (P^). If all goes well and the 

objectives of phases 2 and 3 are met, the place P^ (mission_complete) is reached and the 
mission terminates normally. 

G. CONCLUSIONS 

The conclusion of the work in this chapter has indicated that complex behavior can 
be readily coordinated through Strategic Level rules, that are easily modified. These act as 
state transitioning mechanisms and the communication through Tactical Level software to 
the Execution Level controllers is a simple but convenient way of commanding stable 
responses of the vehicle. The design of well behaved control laws and functions at the 
Execution Level is essential as a primary part of the design and is affected through careful 
attention to the digital control loop design. Reactivity, failure recovery, and even human 
interfacing within the controller can take place at any level. 

Although the example mission here was simplified so that the details of the code 
and results could be more clearly presented, other more complex missions have been 
performed successfully. 
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Figure 3. 1 Perspective View of the NPS Phoenix. 
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Figure 3.2 Major Internal Components of the NFS Phoenix. 
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Figure 3.3 External Components of the NFS Phoenix. 
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Figure 3.4 Section and Front View of the Cross-Body Thruster. 



94 







SIDE VIEW 




Figure 3.5 Rotation Convention for the Control Surfaces. 
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Figure 3.6 Diagram of the Phoenix Networked Controller. 
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Figure 3.7 Structure of the Execution Level Software/Hardvvare. 
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Figure 3.8 Network Configuration for Experiments in the NFS Test Tank. 
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Figure 3.13 The Transition Signal Condition. 
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Figure 3. 14 Structure of the Execution Level Software. 
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Figure 3.15 Communication Interactions Between the Three Levels using 
"exec_submerge " . 
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Figure 3.16 Communication Interactions Between the Three Levels using 
"ask_depth_reached". 
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Figure 3.17 Vehicle Depth, Depth Rate, and Filtered Error vs. Time Response for 
All Three Phases. 
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Figure 3. 1 8 ST 1000 Sonar Image of the NFS Test Tank showing a Cylinder, the Tank 
Walls, and the Vehicle Fins. 
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mission_ complete repeat, done, 
done p2. 
done p3. 

tl. 

pi :-tl. 

p2 pi, askC t2(X)' , X), X == 1. 

p3 pi, askC t3(X)' , X), X == 1. 

ask(Q,A) write(Q), write('?), nl, read(A), nl. 



Figure 3. 19 Petri Net Graph Representation and Prolog Code for Example 1 . 
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mission_complete repeat, done, 
done p2. 

tl. 

pi tl. 

p2 pi, askC t2(X)’ , X), X == 1. 

p2 pi, askC t3(X)' , X), X == 1. 

p2 pi, ask(’t4(Xy, X), X== 1. 

p2 askCt5(Xy, X), X==l. 

ask(Q,A) write(Q), writef?), nl, read(A), nl. 



Figure 3.20 Petri Net Graph Representation and Prolog Code for Example 2. 
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mission_complete repeat, done, 
done p3. 

pi ask('tl(X)’, X), X == 1. 
p2 askCt2(Xy, X), X==l. 
p3 pi, p2, askC t3(X)' , X), X == 1. 

ask(Q,A) write(Q), writeC?), nl, read(A), nl. 



Figure 3.21 Petri Net Graph Representation and Prolog Code for Example 3. 
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Figure 3.22 Petri Net Graph Representation of the Prolog Code for the Generic Mission. 
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Figure 3.23 Petri Net Graph Representation of the Prolog Code for Phase I . 
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Figure 3.24 Petri Net Graph Representation of the Prolog Code for Phase 2. 
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Figure 3.25 Petri Net Graph Representation of the Prolog Code for Phase 3. 
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IV. TRITECH MECHANICALLY SCANNABLE SONARS 



A. GENERAL DESCRIPTION 

In order to investigate the potential for using commercially available, high 
frequency sonar for controlling position of an AUV, the vehicle was equipped with two 
mechanically scannable high frequency sonar heads built by Tritech Corp. One is an ST725 
scanning sonar and the other an ST 1000 profiler sonar. These sonars have been 
successfully tested in the NPS test tank, the NPS swimming pool, and in the harbors at 
Monterey and Moss Landing, CA. Figure 4.1 shows where the sonar units are located on 
the vehicle with the heads located inside a rubber boot at the upper end of the housings. 
Each head can be scanned continuously through 360 degrees of rotation or swept through 
any defined angular sector, around the central axis of the unit. During normal operation the 
head will ping, wait for return echoes to process, and then rotate a specified angular width 
and repeat. The ST 1000 and ST725 sonars measure approximately 9 inches long by 2.75 
inches in diameter. Both heads are powered by 24 Vdc and communicate with the host 
computer using an RS-232 serial link at 9600 Baud. 

1 . ST725 Sonar 

The ST725 sonar operates at a frequency of 725 kilohertz and emits an acoustic 
beam 2.5 degrees wide in the horizontal plane by 24.0 degrees wide in the vertical plane, 
12.0 degrees above and below the horizontal plane as shown in Figure 4.2. This device is 
described as a scanning sonar due to the nature of the range information returned for each 
ping. A scanning sonar operates by placing the intensities of the echoes from each ping into 
discrete segments of range called range bins. For this sonar, the number of range bins is 
nominally 128 but for operating ranges of 10 meters or less, the number of range bins is 
reduced to 64. The maximum operating range of the ST725 is 100 meters with a minimum 
operating range of 6 meters, and provides a resolution of (Maximum Range)/128 or 
(Maximum Range)/64 depending on the range setting used. At the minimum range setting 
used for the majority of the experiments conducted in the NPS hover tank, the smallest 
range resolution was approximately 9 cm. 
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The range bins and associated intensities define a vector of intensities called a 
scanline for a given bearing angle of the head, and is sent through the serial port to the 
controlling computer as a 32 or 64 byte data stream, 64 or 128 bins as appropriate. Each 
byte (8 bit binary) contains the intensity information for two bins, where the first byte, 
(byteO), holds the intensities for binO and binl, the second byte holds intensities for bin2 
and bin3, etc., as shown in Figure 4.3. Since the intensity for each bin is 4 bits wide, the 
intensity value can range only from 0-15 where 15 denotes the strongest return and zero, 
no return. 

In order to more clearly analyze the returns, the data can be thresholded to display 
only returns above a certain intensity value so that significant objects/structures can be 
shown, while other less significant entities (e.g. suspended particles in the water column, 
weak multi-path echoes), are excluded from the display. Combining thresholding with an 
appropriate power setting for the transducer, can usually provide very clear displays. 
Figures 4.4, 4.5, and 4.6 show an ST725 sonar display of the NFS AUV test tank. The 
head was suspended from the vertical catwalk and an aluminum cylinder was placed in the 
tank. Clearly visible are the sides of the tank, with the echoes from the cylinder shown near 
the upper left hand comer of the tank display. A power gain of 13 (0 < gain < 100), and an 
angular step size of 0.9 degrees was used for all three displays, and thresholds 1,10, and 
15 were used for Figures 4.4, 4.5, and 4.6 respectively. Adjustment of the threshold 
shows the filtering ability of this technique. In Figure 4.4, it is clear that the tank walls are 
highly reflective since there is an apparent continuation of each wall beyond the tank 
boundaries. Also, the effect of the finite resolution is indicated by the short segments at 
constant radius emanating from the sonar head location. 

2. STIOOO Sonar 

The STIOOO sonar operates at a frequency of 1024 kilohertz and emits a 1.5 degree 
conical acoustic beam as shown in Figure 4.2, and is described as a profiler sonar since 
only the range to the nearest return echo is recorded. With the use of a high frequency ping, 
imaging of objects to a high degree of detail is possible. Operating ranges for the STIOOO 
sonar are from about 1 meter to a maximum of 50 meters, and the unit is pressure rated to 
full ocean depth. 

For each ping, the sonar head returns to the controlling computer a 2 byte data 
stream, which contains the distance to the first return in units of millimeters. Figure 4.7 
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shows an image of the tank sides, along with two objects clearly visible inside the tank. 
The semi-circular shape in the upper part of the display is an aluminum cylinder standing 
vertically in the water column. In the lower part of the display is a line, which is another 
aluminum cylinder lying in the horizontal plane showing the high degree of detail which 
can be obtained from this sonar. Results from work done by (Ingold, 1992), has shown 
that objects can be imaged to a resolution of less than 2 cm. 

B . COMPUTER CONTROL OF THE SONAR HEAD 

Both sonar heads are rotated with a stepper motor. With a maximum of 400 
steps/rev, the highest angular resolution is 0.9 degrees, although double and triple stepping 
can be enabled resulting in 1.8 or 3.6 degree increments for faster rotation rate. Around the 
stepper motor shaft is a position encoder which returns an ASCII "F" or "T" to the serial 
port each time the motor is stepped. Figure 4.8 shows the locations about the encoder 
where the values of "F" or ”T" are returned. Output from the encoder allows the sonar head 
to be centered at the same location upon initialization, and the count of steps either 
clockwise (CW) or counter-clockwise (CCW) is maintained by the controlling software. 
The exact angular position of the head can then be determined at any subsequent time. 

A procedure for centering the sonar head to it's "ahead position" (i.e. the middle of 
the region of T's) is outlined in Figure 4.9. The procedure shown assumes high resolution 

0. 9 degree steps but the control software has been generalized to also accommodate 1.8 and 
3.6 degree step size settings and is accomplished with a control function called 
"center_sonar". Once the head is centered, the angular position of the sonar head y/^, is 
defined to be 0.0, (-180.0 < y/, < 179.1 degrees), and the angular position count is 
set to 0, (-200 < y/,^ < 199). Each time the head is stepped, y/,^ is updated by either -i-ssiz 
or -ssiz, depending on the direction of rotation. Three step sizes can be programmed, ssiz = 

1, 2, or 4 corresponds to 0.9, 1.8, or 3.6 degrees respectively, which leads to a simple 
relation for calculating the head position in degrees 

¥s = 0.9y/,^ (4.1) 

There is no external feedback of step count so this method of realizing the sonar heading 
assumes that the motor does in fact step on open loop command. The control software 
developed in this dissertation allows three positioning modes of operation for the sonar 
head, which are: 



115 



Mode 0: Ping only mode 



Ping sonar at present heading 
only (do not step). 



Mode 1: Continuous sweep mode 



Ping and step continually 
sweeping in a constant 
clockwise direction. 



Mode -1; Continuous sweep mode - Ping and step continually 



sweeping in a constant 
counter-clockwise direction. 



Mode 2: Sector sweep mode 



Ping each step within a 
defined angular sector. 



Parameter definitions for Mode 2 are shown in Figure 4.10, where y/^j is defined as the 

scan direction, and is the angle that the head position will oscillate with an angular 
amplitude of the sweep width. Mode 1 can be achieved, as a subset of Mode 2 by 
simply specifying y/^j with y/^^. set to 0.0. It thus follows that the values of yr^j, and 
the positioning mode can be dynamically changed at any time as needed according to the 
context of the mission control. 

1 . Sonar Control Functions in Detail 

Both the ST725 and STIOOO sonar heads can be controlled using a small set of 
ASCII character commands sent over the serial line. The sonar(s) do not have automatic 
modes to continuously ping and rotate given a single command. Therefore, the head must 
be commanded through the serial port to ping and/or rotate for each time. Below is a list of 
the commands available for position and pinging control for both heads. 

STIOOO Commands: 

"+" Rotate 1 Step Clockwise without Pinging. Returns 1 Byte Encoder 
Character, "T" or "F". 

Rotate 1 Step Counter-Clockwise without Pinging. Returns 1 Byte 
Encoder Character, "T" or "F". 

"Z" Ping Once Without Stepping. Returns 2 Byte Range. 

")" Ping Once Then Rotate 1 Step Clockwise. Returns 2 Byte Range 
followed by a 1 Byte Encoder Character, "T" or "F". 
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"(" Ping Once Then Rotate 1 Step Counter-Clockwise. Returns 2 Byte 
Range followed by a 1 Byte Encoder Character, "T" or "F". 



ST725 Commands; 



"+" Rotate 1 Step Clockwise without Pinging. Returns 1 Byte Encoder 
Character, "T" or "F". 

Rotate 1 Step Counter-Clockwise without Pinging. Returns 1 Byte 
Encoder Character, "T" or "F". 

"S" Ping Once Without Stepping. Returns 32 or 64 Byte Range Bins. 

"R" Ping Once Then Rotate 1 Step Clockwise. Returns 32 or 64 Byte 
Range Bins followed by a 1 Byte Encoder Character, "T" or "F". 

"L" Ping Once Then Rotate 1 Step Counter-Clockwise. Returns 32 or 64 
Byte Range Bins followed by a 1 Byte Encoder Character, "T" or 



In order to ease operation, several "C" language functions have been written to 
coordinate control of the heads. The following is a list of the functions and parameter lists 
for use in the vehicle Gespac computer. Using the function 

center_sonar(paf/i); 

commands the sonar head rotate to the ahead position, where path = Serial port number, 
and can be used to center either the ST725 or ST 1000. The function 

stl000_sonar_command - set_stl000_mode( 

stl000_sweep_mode, 
stl000_scan_direction, 
stlOOO _scan_width, 

&stl000 _psi_sonar_min, 

&stl000 _psi_sonar_max); 

is used to determine which command to send to the ST 1 000 head based on the parameters 
given by; 

stl000_sweep_mode = Sweep Mode 0,1, -1,2 
stl000_scan_direction = Scan Direction, 0.0. ..360.0 Degrees 
stl000_scan_width = Scan Width, 0.0. ..360.0 Degrees 
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If the sweep mode is 2, the values of stl 000 _psi _sonar _min and 
stlOOO _psi_sonar_max are calculated and returned based on scanjiirection and 
scan_width . If mode 0 is used, stl 000_sonar_command is set to "Z", if mode 1 or 
-1, the commands ")" or "(" are returned. The character "Z" is returned for mode 2 and the 
following function must be called each time step to update the command to the head. 

stlOOO_sonar_command = stl000_sweep_control( 

StlOOO _psi_sonar, 

StlOOO _psi_sonar_min, 

StlOOO _psi_sonar_max)', 

where stlOOO _psi_sonar = Current Angular Position of the head. 

update_stl000_position(5/70<?f>_sonar_eo//imo/i</, 

&stl000 _psi_sonar_count, 
stl 000_ssiz ) ; 

updates the head position s 1 1 0 0 0 _p si _s o na r _c o u nt based on 
stlOOO_sonar_command and stl000_ssiz, where 

StlOOO _psi_sonar_count = Integer count of head position 0...400, incremented by 
+stl000_ssiz if stlOOO_sonar_command is -stl000_ssiz if and 0 if the 
command is "Z". stlOOO_ssiz is 1, 2, or 4 for step sizes 0.9, 1.8, and 3.6 degrees 
respectively. 



ping_stl000_sonar (stl000_sonar_command ); 

sends command stl000_sonar_command to the ST 1000 head. 

stl000_range = read_stl000_sonar(); 

reads the range from ST 1000 and returns it in stl000_range. The pinging and reading 
functions have been separated since other calculations and sensor reading may be 
performed during the waiting time for the sonar return. 

The ST725 uses basically the same functions outlined above but since some of the 
ASCII commands used for this head differ from the STIOOO, different function names 
must be used. A call to the function 
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st725_sonar_command - set_st725_mode( 

st725_sweep_mode, 

st725_scan_direction, 

st725_scan_width, 

&st725 _psi_sonar_min, 

&st725 _psi_sonar_max ); 

determines which command to send to the ST725 head based on the parameters given by: 

st725_sweep_mode = Sweep Mode 0,1, -1.2 
st725_scan_direction - Scan Direction, 0.0. ..360.0 Degrees 
st725_scan_width = Scan Width, 0.0.. .360.0 Degrees 



(NOTE: Same as with the STKXX)) 



If the sweep mode is 2, the values of s t7 2 5 _p si _s o nar _min and 
st725 _psi_sonar_max are calculated and returned based on scanjdirection and 
scan_width . If mode 0 is used, st725_sonar_command is set to "S", if mode 1 or 
-1, the commands "R" or "L" are returned. The character "S" is returned for mode 2 and as 
with the ST 1000, the following function must be called each time step to update the 
command to the head. 

st725_sonar_command - st725_sweep_control( 

st725_psi_sonar, 
st725 _psi_sonar_min, 
st725 _psi_sonar_max); 

where 



st725 _psi_sonar = Current Angular Position of the Head. 



The function 



update_st725_position(s/725_so/iar_comma/i£?, 

&st725 _psi_sonar_count, 
st725_ssiz ) ; 

updates the head position st725 _psi_sonar_count based on st725_sonar_command 
and st725_ssiz, where st725 _psi_sonar_count = Integer count of the head position 
0...400, incremented by +st725_ssiz if st725_sonar_command is "R", -st725_ssiz 
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if "L", and 0 if the command is "S". st725_ssiz is 1,2, or 4 for step sizes 0.9, 1.8, and 
3.6 degrees respectively. The function call 

p\ng_st725_sonstr{st725_sonar_command); 
sends command st725_sonar_command to the ST725 head. Calling 

read_st725_sonar(<fesf725_range [0] ); 
reads the range bins from the ST725 and returns them in array st725_range . 

2 . Sonar Initialization 

Before any sonar data may be collected, the head must be initialized with the 
appropriate parameters for a given operating range. Range settings for the scanning sonar 
(ST725) are from 6 to 100 meters, while for the profiler sonar (ST 1000), the settings range 
from 1 to 50 meters. Tables 4.1, 4.2, and 4.3 list the initialization parameters for these 
ranges, and the sequence of initialization is outlined below using a function, 
send_st725() and send_stl000(), which is used to send characters or numbers to the 
head. Comments and the data type of values to send are to the right. Three different data 
types are used. Char (1 byte ASCII), Byte (1 Byte Binary), and Word (2 Byte Binary). 
The reader can refer to Appendix G for a detailed listing of the standard sonar commands. 



ST725 Head Initialization Sequence: 



1 send_st725(’P’); 

2 send_st725(TxPulse); 

3 send_st725(NSAMPL); 

4 send_st725(NBINS); 

5 send_st725(Range_Code); 

6 send_st725(checksum); 

7 send_st725('X’); 

8 send_st725('K'); 



Send Sonar Parameters (Char) 

Send TxPulse Length (Word) 

Send NSAMPL (Byte) 

Send NBENS (Byte) 

Send Range_Code (Byte) 

Send checksum (Byte) 

Enable Time Varying Gain Mode 
(Char) 

Set mode return Range bin Peak 
(Char) 
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9 send_st725('E'); 

10 send_st725(212); 

11 send_st725('C); 

12 send_st725(gain); 

ST 1000 Head Initialization Sequence: 



Set Final Gain for TVG (Char) 
Gain Based on 0..255 (Byte) 
Set Initial Gain for TVG (Char) 
Gain Based on 0..255 (Byte) 



1 send_stl000('P'); 

2 send_stl000(TxPulse); 

3 send_stl000(NSAMPL); 

4 send_stlOOO(NBINS); 

5 send_stl000(Range_Code); 

6 send_stl000(checksum); 

7 send_stl000(’X'); 

8 send_stlOOO('K'); 

9 send_stl000('E'); 

10 send_stl000(212); 

11 send_stl000('J'); 

12 send_stl000(ECPULS); 

13 send_stl000(TIMOUT); 

14 send_stlOOO(LOKOUT); 

15 send_stl000(ESWAIT); 

16 send_stl000(GECMIN); 

17 send_stl000(GAINDT); 

18 send_stl000(ECSCLX); 

19 send_stl000(ECSCLY); 

20 send_stl000(Maxdst); 

21 send_stl000(DACSCX); 

22 send_stl000(DACSCY); 

23 send_stl000(Rng_unit); 

24 send_stl000(checksum); 



Send Sonar Parameters (Char) 

Send TxPulse Length (Word) 

Send NSAMPL (Byte) 

Send NBINS (Byte) (If in Scanning 
Mode) 

Send Range_Code (Byte) 

Send checksum (Byte) 

Enable Time Varying Gain Mode 
(TVG) (Char) 

Set mode return Range bin Peak 
(Char) 

Set Final Gain for TVG (Char) 

Gain Based on 0..255 (Byte) 

Send Profiler Sonar Parameters 
(Char) 

Send ECPULS (Word) 

Send TIMOUT (Word) 

Send LOKOUT (Word) 

Send ESWAIT (Word) 

Send GECMIN (Byte) 

Send GAINDT (Word) 

Send ECSCLX (Word) 

Send ECSCLY (Word) 

Send Maxdst (Word) 

Send DACSCX (Word) 

Send DACSCY (Word) 

Send Rng_unit (Byte) 

Send checksum (Byte) 
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The duration of the acoustic transmission pulse is defined by TxPulse length 
{fJL sec) as shown in Figure 4. 1 1 . TIMOUT (//sec) is the maximum time to wait for returns 
after the pulse has been sent, and is approximately equal to 2*Max Range/c. LOKOUT 
{fJL sec) is the amount of time for the pulse to travel before meaningful results are obtained, 
which is typically set to 400 /isec. Using this setting, returns from less than about 0.6 
meters from the head will be ignored. The value LOKOUT = (minimum distance)/c. 
ESWATT (/tsec) is the wait time for the head to index in autoechosounder mode (Not 
Applicable). GECMIN is the output power gain equal to 2.55*gain, where 0 < gain < 100. 
Checksum is the lower 8 bits of the sum of the initialization values previously sent. For the 
ST725 and the ST 1000, the values sent in steps 2 through 5 are used, and for the ST 1000, 
an additional checksum is calculated based on the values sent in steps 12 through 23. 



Table 4.1 Initialization Parameters for Scanning Mode (ST725 and STIOOO Heads) 



Operating 
Range (m) 


TxPulse 
(1.96 /isec) 


NSAMPL 


NBINS 


Range 

Code 


6 


30 


1 


64 


0 


10 


30 


3 


64 


1 


20 


100 


3 


128 


2 


25 


125 


4 


128 


3 


30 


150 


6 


128 


4 


50 


250 


12 


128 


5 


100 


475 


26 


128 


7 



Table 4.2 Initialization Parameters for Profiling Mode (STIOOO Head) 



Operating 

Range 

(m) 


TxPulse 

1.96 

fis&c 

units 


NSAMPL 


NBINS 


Range 

Code 


TIMOUT 
(fd sec) 


Maxdst 

(mm) 


1 


30 


1 


64 


00 


1500 


1500 


2 


30 


1 


64 


01 


3000 


3000 


4 


30 


1 


128 


02 


6000 


6000 


6 


30 


1 


128 


03 


9000 


9000 


10 


40 


1 


128 


04 


15000 


15000 


20 


50 


3 


128 


05 


30000 


30000 


30 


75 


6 


128 


06 


45000 


45000 


50 


100 


12 


128 


07 


65535 


65535 
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Table 4.3 Values Common to all Range Settings for ST 1000 Sonar 



ECPULS 


1.96 p sec units 


30 


LOKOUT 


( p sec) 


200 


ESWATT 


1.96 p sec units 


25600 


GECMIN 




2.55*gain 


GAINDT 




64 


ECSCLX 




16383 


ECSCLY 




11374 


DACSCX 




256 


DACSCY 




3125 


Rng_unt 




0 



The initialization sequence previously outlined is performed for both heads using 
the following function; 

\nii\ 2 t\\ZQ_son 2 ir (port, mode, max _range, gain, &Nbins) 



Input Values; 



port 

mode 

max_range 

gain 



RS232 communications port number (int) 
Scanning "S" or Profiling "P" (char) 
Maximum range setting (int) 

Sonar power gain 0..100 (int) 



Output Values; 

Nbins - Number of scan bins to collect 64 or 1 28 
for the particular initialization (int) 



3 . Implementation of Sonar Functions 

The functions previously described are designed to operate in a control loop where 
the sonar control functions are called once per time step. Experience has shown that the 
fastest the sonar can be operated within 6 meters is 0.1 seconds, which allows enough time 
for the head to ping, receive and process a return, then step. Therefore, if both sonars are 
to be used and the time step of the control loop is only 0. 1 seconds, only one sonar may be 
operated per time step. A limitation that can be easily remedied by operating each head once 
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every other time step or simply increasing the time step value. The latter solution may not 
be appropriate if the sonar is used for position feedback in a vehicle control system due to 
stability requirements based on update rates. It has then been suggested that one sonar be 
used during one phase of a mission and then switching to another for a different phase. An 
example of this would be to use the ST725 scanning sonar for large area search and once a 
target of interest has been found, the ST 1000 would take over for more detailed 
imaging/servoing once in close proximity of the target. Another solution is ping the 
sonar(s) at the beginning of the control loop, perform other calculations and then read the 
returns at the end. It is this method that is used with the NFS Phoenix, and the structure is 
outlined below with the following C code fragments: 

st725_port = open(’7t2'\S JWRITE-fS JREAD); 
stlOOO^port = openr/t3'',S_IWRITE+S_IREAD); 

center_sonar(5/725 _port)\ 
center^sonart^/yOOO j)ort)\ 



initialize_sonar(st725_port,’S\st725_inax_range,st725_gain, 

&st725_Nbins); 

initialize_sonar(st 1 000_port,'P',st 1 000_max_range, 

St 1 000_gain,&st 1 000_Nbins); 



START OF MISSION LOOP: 



stl000_sonar_command = set_stl000_mode( 

St 1 000_sweep_mode, 

St 1 000_scan_direction, 

St 1 000_scan_width, 

&st 1 000_psi_sonar_min, 

&st 1 000_psi_sonar_inax); 

st725_sonar_command = st725_sweep_control( 

st725_psLsonar, 

st725_psi_sonar_min, 



124 



st725_psi_sonar_max); 

updaie_st725_position(st725_sonar_command, 

&st725_psi_sonar_couni, 
st725_ssiz ); 

update_st 1 000_position(st 1 000_sonar_command, 

&st 1 000_psLsonar_count, 
stlOOO_ssiz); 

ping_st725_sonar(st725_sonar_coinmand); 

ping_st 1 000_sonar(st 1 000_sonar_command); 



(Perform other calculations while waiting for sonar return) 



read_st725_sonar(&st725_range [0]); 
read_stl000_sonar(&stl000_range [0]); 



st725_psi_sonar = 0.9*st725_psi_sonar_count; 
stl000_psi_sonar = 0.9*stl000_psLsonar_count; 



END MISSION LOOP: 



close(st725_port); 

close(stl000_port); 

Experience with both the sonars on the Gespac computer has shown that the heads 
do not always return the correct number of bytes when requested. Sometimes no, less than, 
or more bytes are returned for a given ping command, and can lead to "locking up" of the 
controlling process if the serial read function attempts to read more bytes than are present in 
the buffer. For example, the program can not simply assume that 3 bytes of data will 
always arrive when the STIOOO head is issued the command "L", which should return 2 
bytes of range data and 1 encoder byte. Occasionally only 2 bytes will arrive for the 
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command, and if the read function attempts to read 3 bytes, the process will hang 
indefinitely since the third, expected byte never arrives. The following code fragment of the 
function read_profile_sonar() outlines the steps taken to ensure robust operation of the 
ST 1000 sonar head. 

read_profile_sonar() 

{ 

n_bytes = _gs_rdy(stl000_path); /* Check How Many Bytes are on The Port */ 
timeout = 0; /* Initialize the Timeout Counter */ 

RESET_PORT = FALSE; /* Assume the Port Does Not Need to be Reset */ 

while(n_bytes < profile_sonar_bytes_expected) /* Loop Until Data is There */ 

{ 

tsleep(2); /* Wait for 0.02 seconds */ 

n_bytes = _gs_rdy(stl000_path); /* Check How Many Bytes are on The Port */ 

/* Reset the Port if an Insufficient Number of Bytes Have Arrived Within the Allotted 
Time */ 

if((timeout == 1) && (n_bytes != profile_sonar_bytes_expected)) 

{ 

RESET.PORT = TRUE; 
break; 

} 

timeout = timeout +1; /* Increment the Timeout Counter */ 

} 

if(RESET_PORT) 

{ 

stl000_range = 0.0; /* Set The Range To Zero By Default Since No Range 
Available */ 

close(stl000_path); /* Close The Serial Port Path */ 

stI000_path = open(7t3",SJWRITE+S_IREAD); /* Reopen The Path */ 

n_bytes = _gs_rdy(stl000_path); /* Check The Newly Opened Port And Check For 

Any Stray Bytes */ 

if(n_bytes != -1 ) n=rcad(stl000_path,x,n_bytes); /* Read Them If There To Ensure a 
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Clean Start On The Next Read */ 



n_bytes = -1; /* Set n_bytes = -1 To Fail If Statements To Follow */ 

} 



if(n_bytes = profile_sonar_bytes_expected) 

{ 

/* Read The Range From The Sonar */ 

) 

if(n_bytes > profile_sonar_bytes_expected) 

( 

/* An Incorrect Number Of Bytes Has Arrived In The Buffer */ 

/* Clear Them Out By Reading Them */ 

stlOOO_range = 0.0; /* Set The Range To Zero By Default *! 

) 

profile_sonar_pinged = FALSE; /* Signify that the Head has Been Read */ 
retum(stlOOO_range); /* Return the Range */ 



Extensive tested of this code has been done, and operates extremely well. No port 
lockups have occurred since it's implementation. 

C. CONCLUSIONS 

This chapter has presented a detailed overview and description of the Tritech ST725 
and STIOOO sonars. Example sonar images have been given along with the data formats for 
both units, along with useful computer functions for controlling and initializing each sonar 
head. 
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CHAPTER IV FIGURES 
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Figure 4. 1 Location of the Tritech Sonar Heads on the NFS Phoenix. 





Figure 4.2 Sonar Beam Shapes for the ST725 and STIOOO Sonars. 
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Figure 4.3 Scanline Data Structure of the ST725 Sonar. 
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Figure 4.4 ST725 Sonar Display of the NFS Test Tank for Threshold = 1 . 
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Figure 4.5 ST725 Sonar Display of the NFS Test Tank for Threshold = 1 0. 
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Figure 4.6 ST725 Sonar Display of the NFS Test Tank for Threshold = 15. 
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Figure 4.7 STIOOO Sonar Display of the NFS Test Tank. 
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Figure 4.8 Sonar Head Encoder Definitions. 
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Figure 4.9 Method to Align the Sonar Head to the Ahead Position. 
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Figure 4. 10 Sector Sweep Mode Variables. 
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Figure 4.1 1 Transmitted Pulse Description. 
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V. CONTROL SYSTEM EVALUATION 



Results from a series of in-water experiments using the Phoenix underwater vehicle 
are presented in this chapter. All tests used the tri-level control structure described in 
Chapter in, and were performed in the NPS test tank. The series was conducted to evaluate 
the performance of sliding mode control designs for submergence, rotation, and 
longitudinal positioning of the vehicle to a prescribed stand-off distance from an object. 
These are typical maneuvers that an AUV will be required to perform for ocean intervention 
and inspection tasks. Since the high level controller must rely on a stable, and well tuned 
autopilot to perform maneuvers, the closed-loop servo level controllers must exhibit 
predictable and well understood behavior. This then frees the mission designer to focus on 
specific mission objectives. 

The chapter opens with a discussion of the experimental setup, which includes 
descriptions of the test tank, computers and network, and is followed by a brief explanation 
of the steps taken to prepare the vehicle for testing. The second section addresses the issues 
surrounding the design of a software filter for depth rate estimation, using signals from the 
vehicle pressure transducer. This analysis leads to a determination of the best filter design 
as the basis for "in vehicle" implementation and experimental validation. 

The next section contains results from submergence, rotation, and longitudinal 
motion control experiments. The effects of varying the sliding mode control gains and 
system parameter estimates for each control mode are presented and the best values to use 
are determined. The pertinent sections of the Strategic Level Prolog codes used for each 
experiment are given and explained. Following this are results from simultaneously 
submerging and rotating the vehicle to a specified depth and heading. The controller for this 
maneuver utilizes command generators, which will ensure that the desired position and 
heading will be achieved at the same time. 
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A. 



EXPERIMENTAL SETUP 



1 . NPS AUV Test Tank 

All vehicle experiments were conducted in the NPS test tank, which is constructed 
of 1/4" steel plates, measures 20 feet square with an open top, and is filled with fresh water 
to a depth of five feet. The water is continuously circulated and chemically treated using 
readily available swimming pool pumps and filters. The tank stands on the floor and has 
two Plexiglas windows located on the sides for observation. To enable ease of vehicle 
manipulation by laboratory personnel, a cat walk is located across the top of the tank. 
Placement and retrieval of the vehicle from the water is achieved by using an electrically 
powered hoist that is attached to a movable carriage, which rolls along a beam that extends 
across the entire length of the tank. Gridlines are marked on the tank bottom and sides to 
provide a reference background to improve observation of vehicle motions. 

2 . Computer Network 

The computer network configuration used for the experiments is shown in Figure 
3.8. It consists of five nodes connected by thinwire ethemet for network communications 
using TCP/IP. This arrangement allows communications between any of the nodes at any 
time, even while the experiment is ongoing. Since two computers are needed to implement 
the tri-level control structure, the Execution Level, residing in the Gespac must 
communicate with the system running the Strategic and Tactical Levels (Sun SPARC). At 
the time these experiments were performed, only the Gespac computer was located onboard 
the vehicle and was connected to the rest of the network using a water-proof, thinwire 
connector. 

For missions using the STIOOO sonar, an SGI Elan workstation is used for 
displaying, in real time, the sonar data collected in the Execution Level. Installed on the 
DOS PC is a cross-compiler/linker which converts C language source code files into 
executable images which will operate on OS9 based systems. It is here that the Execution 
Level software is compiled and linked. The source code is usually developed on the SGI 
Elan or equivalent system. A radio ethernet unit is used for network connections to the 
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Internet, and has made data transfer between facilities very fast and convenient, especially 
if software is developed on remote systems. 

3 . Pre-Mission Procedures 

Before an experiment can be performed, the required software must be present in 
each computer and the network communications enabled. Since the Gespac computer has 
no hard disk, and uses a volatile ram disk for data and program storage, each time the 
system is powered, the Execution Level software must be reloaded. The data transfer is 
done through the network using ftp (File Transfer Program), usually from the PC, since it 
is there that the software was cross-compiled. Upon power up, the Gespac boots from a set 
of EPROM's located on the CPU board which contains the system boot file, operating 
system, and network software modules. Once the boot process is complete, a ramdisk is 
created and the network is automatically started allowing connections from remote systems 
possible. The SPARC station on the other hand, boots from it's internal hard drive, where 
the Strategic and Tactical Level software already resides, so no program loading is 
required. 

The Execution Level program is not the only software that needs to be loaded into 
the Gespac. Various modules known as "device descriptors" must be also present to 
provide an interface between application programs and hardware devices. Descriptors for 
the sonar and DiveTracker serial ports, and configuration information for the creation of 
additional ramdisks are required. These modules are transferred along with the Execution 
Level software using an ftp script known as "exec. ftp", which allows all modules to be 
downloaded using a single command. Once this is complete, the Gespac system is ready 
for the Execution Level program to start. At this point, the procedures for mission 
execution outlined in Chapter HI are performed. 

B. SUBMERGENCE CONTROL 

In this section, vehicle submergence control using the two vertical thrusters is 
discussed. Here, the term submergence control is used rather than "depth control", to 
distinguish between the use of vertical thrusters rather than fins, which in a normal flight 
mode would be used to control depth. No forward motion is assumed, so the control 
planes are ineffective in this scenario. The sensor used for this mode is a depth cell utilizing 
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a pressure transducer. Since proper positional control of the vehicle requires some form of 
rate feedback, a digital software filter must be designed to smooth the noise contaminated 
depth measurement and extract the rate of change of depth from this signal. 

1 . Filter Design 

The purpose of the filter is twofold. Firstly, and most important, an estimate of the 
depth rate must be extracted from the primary sensory output, and secondly, depending on 
the circumstances, noise in the primary sensor must be smoothed. In this section, a 
discussion of the effects of filter bandwidth is given first, and examined through the use of 
hard experimental data from earlier maneuvers of the NFS Phoenix. Thus realistic noise 
levels are taken into account. 

The filter model is based on a three state kinematic model for depth changing 
excited by acceleration noise, 



i/(0 = Z2it) 

Z2(t) = Zjit) (5.1) 

Zsit) = q(t). 

The states £,(/), Z 2 (t) , and £ 3(0 are estimates of the position, velocity, and acceleration 
of the depth signal z(t). The measurement equation with z(t) being the depth signal is 

z(t) = £,(?) + v(t) (5.2) 

where q(t) is taken to be the covariance of the system noise and v(t) is the covariance of 

the measurement noise. These equations are the basis for the formulation of a Kalman filter 

(Gelb, 1988). The values of q(t) and v(t) must be carefully chosen so the bandwidth of 
the filter is appropriate for the nature of the depth signal. If the system noise was large, 
with low measurement noise, the filter response would be quite fast. Decreasing the system 
noise and increasing the measurement noise will create the opposite effect and will result in 
a sluggish slow responding filter. The depth signal does contain some noise, specifically 
due to quantization of the analog pressure transducer voltage to it's digital representation in 
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the controlling computer. Therefore the filter must be tuned for the quantization resolution 
of the analog to digital converter. 



The state space representation of Eqns. (5.1) and (5.2) is 

lit) = Az{t) + Bq{t) 
z(t) = Cz(t) + v(t). 



(5.3) 

(5.4) 



where 



and the covariances, 



z(t) e SI 



3x1 



is assumed to be of zero mean, Gaussian white noise, uncorrelated, and 



, C = [7 0 0 ] 





~0 1 O' 




'O' 


A = 


0 0 } 


, B = 


0 




0 0 0 




I 



For implementation of the execution level control functions, data is sampled at a 
fixed rate, 10 Hz, and it follows that the discrete time approximation of Eqns. (5.3) and 
(5.4) is required. The discrete time formulation using a sampling time of 0.1 seconds is 



+ ^^k 


(5.5) 


Zk = Hzi^ + Vk 


(5.6) 
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where 





1.000 0.100 0.005' 




'0.0002' 


0 = 


0.000 1.000 0.100 


, r = 


0.0050 




0.000 0.000 1.000 _ 




0.1000 



With the model defined, an optimal estimate of the state conditioned upon data 
Z^, can be obtained using a Kalman filter as based on a recursive weighted least-squares 
solution. (Developed in more detail in (Gelb)). The equation for the state estimate is 

Zk = h + hiZk - Hzk) (5.7) 

where Zk is the kinematic model based expected value of conditioned upon a prior 
estimate without correction from the measurement Zk- is obtained from 

z, = <^>£,.y, (5.8) 

and the correction is based on the residual - /fZk , multiplied by the optimal (minimum 
variance of the estimation error) gain. 

The Kalman filter gain is then given by 

Lk = MkH\HMkH^ + V,)-' (5.9) 

where is the model based propagated state error covariance matrix 

M, = (5.10) 

and Pk is it's corrected value after measurement and is given by 

P, = M, - + Vk)~'HM^ (5.11) 
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Inspection of Eqns. (5.9) and (5.10) shows that one matrix inverse computation 
involving is required each time step to update the estimate of the state which can 

consume valuable computing time. Fortunately, experiments with real data have shown 
(Figure 5.1), that after an initial settling time, the gains reach steady state values and, as 
such, may be pre-computed and stored as data rather than requiring a step by step update. 

Three filter designs have been studied and are presented to illustrate the effects of 
varying the system and measurement noise. The filters are applied to actual experimental 
data obtained from the depth cell for a dive of 2.0 feet, as shown in Figure 5.2. The 
assumed noise levels for the three designs are as follows 



Filter 1 (Slow) 




= 0.1, 


= 10.0 


Filter 2 (Medium) 


^k 


= 0.1, 


V, =0.01 


Filter 3 (Fast) 


^k 


= 10.0, 


Vj =0.001 



which lead to bandwidths of approximately 0.2, 0.6, and 1.7 Hz respectively. The 
associated steady state filter gains are: 





0.0887' 




'0.2544 




0.6042' 


Filter 1 L = < 


0.0411 


Filter 1 L = ' 


0.3727 


' , Filter 3 L = < 


2.7513 > 




0.0095 




0.2731 




6.2909 



Results showing the estimated depth and depth rate for Filter 1 is shown in Figure 
5.3. Comparing the depth estimate with the raw signal show this filter is too slow as 
evident from the lags present. The response is slightly oscillatory and does not track the 
real signal very well, and the rate also shows some oscillatory behavior during the steady 
state interval. Figure 5.4 shows the response for Filter 2, and displays a marked 
improvement over the previous filter, because the depth estimate follows the raw signal 
very closely, although the rate estimate shows some sign of the sensor noise. Filter 3, is 
shown in Figure 5.5, and again the depth estimate closely follows the raw signal, but the 
rate estimate is very poor since the quantization noise is strongly imparted to this estimate. 

To more fully understand the performance of the three filters, a frequency response 
analysis has been performed. Figures 5.6 and 5.7 show the log-magnitude and phase angle 
plots for the depth rate estimate of the three filters. It can be seen from the plots that Filters 
1, 2, and 3 exhibit increasing bandwidths. Although Filter 3 has the largest bandwidth it is 
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so large that it is faithfully reproducing the noise of the sensor and should not be used on 
these grounds. The Medium filter has a smaller bandwidth and is much less affected by the 
noise, and is the filter of choice for depth control. Since the filter is relatively slow, it will 
be an important factor in the sliding mode control design to follow. 

2 . Submergence Control Simulation 

Now that three filters have been designed, each may be evaluated for estimating the 
depth and depth rate using a dynamic simulation of the Phoenix vehicle under submergence 
control. A sliding mode controller will be designed which will require the depth and depth 
rate for feedback. The control inputs are the two vertical thrusters, which, in the 
simulation, act together as a single input causing the vehicle to be confined to one direction 
of travel. Motion occurs along the body-fixed z-axis, which is also assumed to remain 
parallel to the global Z-axis and the gravity vector. With these restrictions, and no vertical 
current component such that vv(/) = z(/), the continuous time dynamic model for depth 
motion becomes 



where 



M,z{t) + bMt)\z(t)\ ^ Fb = 2a.\it)\\(t)\ 
= in -f . 



( 5 . 12 ) 



and is the heave added mass, and a. is a coefficient relating the square of the vertical 
thruster motor voltage, \(t), to the force developed. The thruster effectiveness is doubled 
since both thrusters are operated simultaneously and receive equal control voltages. Fg is 
any net force acting on the vehicle caused by a deviation from neutral buoyancy, and is 
the coefficient of square law drag in the vertical direction. 



The position and rate errors for submergence control are defined as 

2(0 = - z{t) 



2(0 = - z{t). 



( 5 . 13 ) 



150 



where the subscript "com" refers to the commanded depth or depth rate. The sliding surface 
for a single input/output control design is a scalar equation given by 

CT(/) = i{t) + Az(0 (5.14) 

where A is scalar quantity analogous to the matrix described in Chapter II. The control 
voltage can be expressed as 



v(0 = ^|\y{t)\sgn(yit)) 



(5.15) 



where 



7(0 = 



M, 



2a, 



.(0 + 



b^z(t)\z{t)\ 

K 



F 

+ -J- + Az(0 + rjsat{ait) / (j)) 
M, 



A 

y 



Since only the depth signal is available for measurement and we wish to reduce the 
computation time for the state estimate, the constant gain filter will be used to supply depth 
rate feedback for the sliding mode controller. With this, the control equation uses the 
estimates of depth and depth rate as follows: 



7(0 = 



- f 

M, . 

2a, 



AO + 



b.z{t)z{t) 



+ -7^ + Az(0 + rjsat{d(t) / <j)) (5.16) 

M, 



where 








CT(0 = z(0 + Az(0 


(5.17) 


and 








z(0 = z,on,(t) - z{t) 


(5.18) 



Figure 5.8 shows the response to a step command of 4.0 feet in depth using the 
three filters previously designed. Depth rate and commanded control voltage levels are 
shown in Figures 5.9 and 5.10 respectively. The control gains A, Tf, and the boundary 
layer thickness ^ were varied until the most satisfactory response was observed as a 
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balance between tracking error and control activity. The appropriate gains for each filter 
were 



Filter 1 A = 0.1 , rj = 0.1 , = 0.7 

Filter2 X = 0.2 , T] = OA , (j) =0.2 

Filters X = 0.5 , T] = 0.1 , (p =0.1 



The slower the filter, the lower the control gains were to compensate for filter lags, 
and is reflected by the speed of response of the respective systems. The boundary layer 
thickness also had to be broadened with decreasing filter bandwidth. If high gains and a 
small boundary layer thickness is used, the system will become unstable for slower filters 
because of the time lags. 

The system was most sensitive to boundary layer thickness. For small errors, <7(t) 
is within the boundary layer, and with a small value of the switching function slope is 
sharp, providing high gain control for small errors. The switching curve is shown in 
Figure 5.1 1 contrasting the wide boundary layer, ^ = 1.0, with a narrow boundary layer, 
(j) =0.1. These three cases have shown that the selection of the control gains is highly 
dependent on the bandwidth of the Kalman filter used. Therefore, the controller must be 
tuned for each specific filter design. 

3 . Variable Boundary Layer Formulations 



To avoid the task of selecting unique gains for each sliding mode controller, a 
method to vary the values of the gains can be used. Sensitivity to the boundary layer 
thickness can be overcome by formulating ^ as a function of < 7 (/). It is desired that the 
gain is low near < 7(0 = 0 and high for larger values of < 7 (/). One way to accomplished this 
is using a linear function, such as 



= <Pmax 






(5.19) 
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or a non-linear function 






^max 

(<Pmax - <Pmin)^(0 



+ 1 



min ^ max 



(5.20) 



where is the maximum value C7(t) will assume, or in the case of a step input, the 
value of <j(t) at maximum error, <T(0). The curves for the linear and non-linear functions 
are shown in Figure 5.12. was chosen as 1.0 with =0.1. Returning to Figure 
5.1 1 the two switching curves for the two functions for </>(cr) are shown. From this, it is 
easily seen that the gain will be equivalent to using a constant value of 0(cr) = 1.0 near 
G{t) = 0 but the gain increases faster with increasing G{t) than if a value of 0(cr) = 1.0 is 
used throughout. The increase in gain is even more accentuated using the non-linear 
function. This method can also be extended to adapt the values of A and r\ to reduce the 
amount of controller tuning required. 

To evaluate the effectiveness of the variable boundary layer, the sliding mode 
controller Eqn. (5.15) was modified to use Eqn. (5.19) for the calculation of 0. For 
purposes of comparison, the response to a step input of 1.0 foot using a constant, 0 =0.1, 
and variable boundary layer is shown in Figure 5.13. It can be seen that when using a 
constant value of 0, a limit cycle instability appears once the set point is reached. 
However, the response is stabilized if a variable boundary layer is used, decreasing the 
gain near the set point were the tracking errors are small. 



4 . Evaluation of Control Law Robustness Through Parameter 
Sensitivity (Step Response) 



With the computer simulation studies completed, the controller designed above was 
implemented on the Phoenix in the test tank. Each control gain or vehicle parameter was 
individually varied from the nominal values listed in Table 5.1, which were obtained from 
the simulations to determine their effectiveness on vehicle performance. The vertical 
thrusters were the only actuators used and the forward speed was zero. Each test used a 
step input of depth error with = Zc„„ = 0. The test cases were separated into four 

distinct series which are shown in Tables 5.2 through 5.5. 
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Table 5.1 Nominal Controller GainsA'ehicle Parameters 



M, 


A 

a. 


V 


X 


0 


A 

b, 


lb - sec^ 


lb 


ft 


rad 


jl 


lb - sec^ 


ft 




sec^ 


sec 


sec 


ft' 


21. Q 


0.004 


0.10 


0.2 


0.2 


28.8 



A 

Table 5.2 Controller GainsA^ehicle Parameters for Vertical Damping, b^. Test 
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V 
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0 


4 


(fi 
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27.0 


0.004 


0.10 


0.2 


0.2 


10.0 


3.0 


2 


27.0 


0.004 


0.10 


0.2 


0.2 


28.8 


3.0 


3 


27.0 


0.004 


0.10 


0.2 


0.2 


45.0 


3.0 


4 


27.0 


0.004 


0.10 


0.2 


0.2 


90.0 


3.0 



Table 5.3 Controller GainsA^ehicle Parameters for Thruster Effectiveness, Test 





M, 




n 


X 


0 


k 


(/O 


1 


27.0 


0.002 


0.10 


0.2 


0.2 


28.8 


2.0 


2 


21. Q 


0.004 


0.10 


0.2 


0.2 


28.8 


2.0 


3 


27.0 


0.008 


0.10 


0.2 


0.2 


28.8 


2.0 



Table 5.4 Controller Gains/Vehicle Parameters for Switching Gain, rj. Test 
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M. 
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Zc«m ift) 
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27.0 


0.004 


0.05 


0.2 


0.2 


28.8 


2.0 
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27.0 


0.004 


0.20 


0.2 


0.2 


28.8 


2.0 


3 


27.0 


0.002 


0.20 


0.2 


0.2 


28.8 


2.0 
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Table 5.5 Controller GainsA/^ehicle Parameters for Sliding Surface Position 
Error Coefficient, A , Test 





M, 




n 


A 


0 


k 


ifO 


1 


27.0 


0.004 


0.10 


0.1 


0.2 


28.8 


2.0 


2 


27.0 


0.004 


0.10 


0.2 


0.2 


28.8 


2.0 


3 


27.0 


0.004 


0.10 


0.4 


0.2 


8.8 


2.0 



The of portion of the Strategic Level Prolog used for the submergence control 
experiments is given below. 



execute_phase(2) exec_next_setpt_data(X), exec_submerge(X), 

exec_start_depth_error_filter(X), exec_start_timer(X), 
repeat, phase_completed(2). 

phase_completed(2) exec_sleep(l,X), ask_depth_reached(X), X==l, 

asserta(complete(2)). 

phase_completed(2) ask_time_out(X), X==l, exec_surface(X), 

printscCPHASE 2 ABORTED DUE TO TIME OUT!’), 
repeat, ask_surface_reached(X), X==l, 
asserta(abort(2)). 



Note that vehicle initialization is performed during phase (1), and the submergence test 
occurs during phase (2). Running this section of code instructs the Tactical Level to send 
the depth set point, control mode, and phase timeout information to the Execution Level 
that is specified in the mission file (exec_next_setpt_data). Once this has been done, the 
vehicle is commanded to submerge (exec_submerge) and have the depth error filter started 
in the Execution Level (exec_start_depth_error_filter). At this point, the timeout counter is 
started in the Tactical Level and the query predicate, "ask_depth_reached" is repeatedly 
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executed until the commanded depth has been reached. If the set point is not attained before 
the time out , an abort is ordered and the vehicle surfaces. 

The results for varying the vertical damping coefficient, b^, are shown in Figures 
5.14 through 5.16. It is evident from the depth response that the damping coefficient used 
in the controller directly affects the speed of response. Using relatively small values results 
in lower command voltages on the thrusters since the controller is assuming a very lightly 
damped system exists. Increasing the damping to relatively large values produces the 
opposite effect and the control voltages are very high. In fact, the results from the fourth 
test, = 90.0 lb- sec^ / , caused the thrusters to saturate, and the vehicle struck the 

bottom of the tank and bounced upwards before being controlled to the set point. 

The next series involved varying another vehicle parameter, the thruster 
effectiveness coefficient, a,, and these results are shown in Figures 5.17 and 5.18. 
Overdriving of the thruster can be clearly seen in the upper trace of Figure 5.18. Using a 
larger value for 6c, causes a much softer control action since a very "strong" thruster is 
assumed, as shown in the lower trace of the figure, and was so small that the vehicle had 
difficulty reaching the set point. 

The switching gain, 7], was the next value to be investigated and these results 
appear in Figures 5.19 and 5.20, showing the effects of varying the control gain as 
opposed to vehicle parameters. A limit cycle is seen for the largest value of rj since the 
slightest error is strongly amplified. The third test shows an even larger amplitude limit 
cycle since the thruster effectiveness, cc,, was lowered from the nominal resulting in an 
even higher gain. Using a small gain provided a very soft control preventing the set point to 
be reached. 

The final series analyzed the effectiveness of the sliding surface position error 
coefficient. A, shown in Figures 5.21 and 5.22. These results followed those of the 
switching gain series since both the values of A and 7] affect the overall gain of the control 
system. However, the contribution of A , which is part of the sliding surface definition, to 
the gain is attenuated by the saturation, (p- This is reflected by smaller amplitudes of 
control action oscillation compared to those generated by rj . 

The oscillatory control voltage behavior seen in the previous results is not solely 
caused by high gain. One reason is the fact that in the steady state, there is virtually no 
damping force present due to the low velocity. Another cause is the sensitivity of the square 
root function in Eqn. (5.15). For values of x{t) near the origin, which reflect small errors, 
the control voltage is more strongly amplified than in other regions. The coarse 



156 



discretization resolution from the depth sensor channel is causing the velocity estimate from 
the filter to be very noisy and the controller is excited by this. The resolution of the depth 
measurement was only 0.0182 feet which was mainly due to the large operating range of 
the depth cell (0 - 37.0 ft). The A/D converter had a digital resolution of 0-2048 for an 
input voltage range of 0 - 10 Vdc which is proportional to the depth range. This relatively 
coarse resolution can be improved by either using a higher resolution A/D unit or selecting 
a depth cell with a smaller operating range and comparable output voltage. 

5 . Submergence Integral Control 

So far the analysis has assumed that the value of the net buoyancy force Fq in Eqn. 
(5.12) is zero. In practice, this is usually not the case since achieving complete neutral 
buoyancy of an actual vehicle is very difficult given the variability of water temperatures, 
salinity, and the hull compression effects of increased depth. Also it is usually advisable to 
ballast an underwater vehicle slightly positive (light) since if the power or control systems 
fail, recovery will be possible. The consequence of a positively or negatively ballasted 
vehicle is that a steady-state offset from the commanded depth will result. Figure 5.23 
shows the simulated depth response for a light, heavy and a neutrally buoyant vehicle for a 
commanded depth of 2.0 feet. The disturbance can be satisfactorily handled using integral 
control. 

The sliding surface equation, (5.14) can be modified to include an integral term 
given by 



G{t) = ?(0 -i- X,z{t) + Ajz(r)Jr, (5.21) 

where the integral term should not exceed a pre-defined maximum value, , such that 

\\l{t)dt\ < 1^. 



Limiting the growth is referred to as anti-reset windup, and will be discussed later in this 
section relating to position control performance. 
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The new control for thruster volts becomes 



v(0 = 



(5.22) 



where 

7(0 = + A,z(r) + A_,z(0 + / 7 tanh(o-(/)/ + -^(4^(0|i(0| + 

The addition of the term XjZiO will remove any steady state depth errors from the 
response, but for best results, integral control should be applied only during certain phases 
of the maneuver. 

Figure 5.24 shows the simulated depth response using integral control from time 
t = 0 until t - 120 seconds. Since the position error is large at the start of the maneuver, 
the integral grows very rapidly and contributes a large control force to submerge the 
vehicle. The control action persists beyond the set point, resulting in an overshoot that is 
especially large for the heavy case, and regardless of the anti-reset windup, an overshoot 
must occur to subtract from the growth of the integral. One solution to this problem is to 
activate integral control only after a steady state offset is detected, as shown in Figure 5.25 
(i.e. error closure has not been achieved within a preset time). Using this approach greatly 
reduces any overshoot for the heavy case and for a light vehicle, no overshoot will occur. It 
should be noted that if the anti-reset windup limit, is not large enough to overcome 

the net buoyancy force, F^, a steady state error will exist proportional to the difference in 
these values. 

The in water results of the NFS Phoenix are shown in Figures 5.26 and 5.27 for a 
commanded depth of 2.0 feet. The depth response for a lightly, partially lightly, and 
heavily ballasted vehicle is shown, along with the commanded control volts to the vertical 
thrusters. Varying the buoyancy was achieved by adding lead weights on top of the hull to 
increase the weight for each respective experiment. If the commanded depth was not 
achieved within 40 seconds, integral control was activated and the errors were significantly 
reduced as shown. The control voltages reflect the buoyancy condition that exists since the 
steady state voltage for the light cases is positively biased providing an overall downward 
force, while the opposite is true for the heavy case, which commands an upward force. If 
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the vehicle is neutrally buoyant, the set point depth will be achieved within the specified 
time and no integral action will be required and is not activated. 

6 . Command Generators for Precision Depth Maneuvering 

So far all depth changes have been performed using step inputs of position error for 
the control input, leaving the velocity and acceleration commands zero, leading to a large 
initial thrust requirement and actuator saturation. Using softer control gains can alleviate 
this, but usually causes very sluggish response with poor steady-state tracking precision 
and disturbance rejection. To control the transient response more precisely while 
maintaining adequate bandwidth, two forms of command generators have been formulated 
which consist of the desired position, velocity, and acceleration of the vehicle depth as a 
function of time. Form 1 requires that the maximum velocity and acceleration be specified, 
along with the desired depth change and the time to complete the maneuver is dependent on 
these values. Form 2 allows specification of both the final depth and time to complete the 
maneuver but due to the added constraint, the maximum velocity can not be chosen 
arbitrarily. For the submerge motion, the two command generators are taken from 

Zj, ^nuijr’ (Form 1) 

or 

[ ^com’ ^com’ ^com J ^(^0’ ^/’ ^max’ '^0’ (Foilll 2 ) 

where a fifth order, zero jerk profile has been chosen so that the maximum acceleration and 
the bandwidth capacity of the vehicle is not overly exceeded and Tq is the initial depth 

and starting time while Zf, Tj is the final desired depth and time at the end of the 

maneuver. Using this technique significantly reduces the occurrence of actuator saturation, 
since the errors are continuously small throughout the depth change phase. A detailed 
description and the equations used are presented in Appendix B. 

Experimental results for both the command generator formulations will now be 
presented. The conditions were identical to those used for the step response tests except 
that the initial depth before the maneuver begins was not at the surface but approximately 
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0.75 feet under. By doing this, any surface effects are minimized that may interfere with 
the submerging ability of the vehicle. 

Figures 5.28 through 5.30 show the results using command generator Form 1. The 
nominal control parameters in Table 5.1 were used with a commanded depth 2.0 feet and 
^TOLt’ ^max "'^re 0.02 ft / sec and 0.006 ft / sec^ respectively. It can be seen from the 

position response, that the commanded depth is not being tracked very well until the very 
end of the maneuver. The control voltage trace shows a great deal of oscillation caused not 
only by the velocity noise, but when using a command generator, the position and velocity 
errors are small throughout the maneuver, giving rise to the high gain effect of the square 
root function discussed earlier. Since using a higher gain controller to improve tracking 
performance caused even more control action, it was decided to relax the command 
generator specification which is presented next. 

Results from Form 2 of the command generator is shown in Figures 5.31 through 
5.33. where a final depth of 3.0 feet was specified and the elapsed time to complete the 
maneuver was 60.0 seconds. It was desired to use the smallest possible given the 

distance and time constraints, which from Appendix B is 



= 8 - 



K/ ^0 



{Tf - 



(5.23) 



and computes gentler profiles for the vehicle to follow and results in a much improved 
response. It should be noted that a slight overshoot occurs near the end of the maneuver 
due to a heavy vehicle that day but it is quickly removed by the integral control action 
which was activated once the error was detected. The velocity command was also tracked 
very accurately. 

7. Conclusions From Submergence Control Studies 

The results presented have shown that the depth of the Phoenix can be precisely 
controlled using Sliding Mode control of the vertical thrusters. Although discretization 
noise from the depth cell produced unfavorable control action, the overall performance is 
exceptional and this problem can be easily rectified using a better sensor. Applying integral 
control has shown to be very effective in compensating for any deviation of the vehicle 
from neutral buoyancy, especially if activated at the proper time. Using command 
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generators provided extremely precise, time based maneuvering, even in the presence of 
buoyancy disturbances. 

C . HEADING CONTROL 

Described in this section is the development of the sliding mode heading control 
equations for the Phoenix vehicle. The two lateral thrusters are the only actuators used and 
the nominal forward speed is zero. All experimental data was collected in the NPS AUV 
test tank using the submergence controller developed previously for depth control. The 
sensors used for control feedback are the directional and yaw rate gyroscopes. The first 
section will present the control algorithm development, followed by experimental results of 
step response for heading command. The final section shows the results for command 
generator inputs to the controller specifying a time based desired angular position, velocity, 
and acceleration for a maneuver. 

1 . Vehicle Model for Heading Control 

The vehicle dynamics for rotation about the body-fixed z-axis (yaw) using both the 
bow and stem lateral thmsters can be described by the following differential equation for 
the continuous time, continuous state evolution: 

/^V>(t) + = 2a^v(r)|v(r)| -h (5.24) 



where 



and is the added inertia about the z-axis, is a coefficient relating the square of the 
lateral thmster motor voltage, \(t), to the moment developed, is the coefficient of 
rotational square law drag, and is the force error. It has an upper bound, y, such 

that < 7, where L is the L, norm of Since this development assumes 

motion restricted to a horizontal plane with depth control assumed by a separate controller, 
r(t) = \j/(t) and r{t) = 
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The angular position and rate tracking errors are defined as 



¥(0 = ¥ com (0 - ¥(0 



¥(0 = ¥c.JO - ¥(0, 



( 5 . 25 ) 



with the sliding surface given by 



= ¥¥) + ^y,¥(t)- 



( 5 . 26 ) 



The sliding mode control law for rotational control is given by 



V(0 = ^l7\{t)\sgn(y{t)) 



where 



( 5 . 27 ) 



= It 



¥con, + ^s>¥¥) + y >^(0|>/(0| + ri^tanhio^it)/ <p^) - 



^ J 



Since both the bow and stem lateral thrusters are equidistant from the vehicle center and are 
of equal size and power, the magnitude of the solution to (7.2) is commanded to each 
thmster except for a difference in sign such that 

V/,/,(0 = + v(r) 

and 

v,v„(0 = - v(r) 

where v^,,(r) and v^,,(r) are the voltage commands to the bow and stern lateral thrusters 
respectively. 
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2 . Heading Control Experimental Results (Step Response) 



Results obtained from in water testing of the heading control will now be presented. 

Four majors series are shown with the first being the response to varying and observing the 
effects of the switching gain 77 ^, the second varying the switch saturation level 0^, the 

third involves adding an output filter and dead zone to the control voltage command for 
further smoothing. The final series presents the results of using time based command 
generators for precision control to a prescribed heading. All step response tests used 

¥com = Wcon. = 0 . 

The Prolog code used to perform this is given by 



execute_phase(3) exec_next_setpt_data(X), exec_submerge(X), exec_rotate(X), 

exec_start_timer(X), repeat, phase_completed(4). 

phase_completed(3) exec_sleep(l,X), ask_depth_reached(X), X==l, 

ask_heading_reached(X), X==l, 
asserta(complete(3)). 

phase_completed(3) ask_time_out(X), X==l, exec_surface(X), 

printscC’PHASE 3 ABORTED DUE TO TIME OUT!'), 
repeat, ask_surface_reached(X), X==l, 
asserta(abort(3)). 



Each test is started by submerging the vehicle to a depth of 2.5 ft using the 
submergence control method outlined previously to minimize surface effects during rotation 
(performed during phase 2). Phase 3 begins with the Tactical Level sending the depth and 
heading set points, etc. to the Execution Level. If the commanded heading is not reached 
before the timeout, an abort is declared and the vehicle surfaces. 
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A nominal set of control gains and vehicle parameters were obtained through 
computer simulation and parameter identification of the rotational motion which are given in 
Table 5.6 below (Torsiello, 1994). 



Table 5.6 Nominal Parameters and Gains for Heading Control 



Parameter/ 

Gain 


1 


K 






ri^ 


K 


Unit 


Ib-ft-sec^ 


lb- ft-sec^ 


lb- ft 


rad 

sec 


rad 

sec^ 


rad 

sec 


Value 


80.00 


55.87 


0.006 


0.200 


0.010 


0.010 



The first series of tests were run to study the effects of different values of the 
switching gain The parameters used in the controller are listed in Table 5.7 and the 

results shown in Figures 5.34 through 5.36 for a commanded heading of k/2 radians 
from an initial heading of 0.0. 



Table 5.7 Parameters for Switching Gain Series 



Case # 


h 


K 








K 


1 


80.00 


55.87 


0.006 


0.200 


0.010 


0.010 


2 


80.00 


55.87 


0.006 


0.200 


0.050 


0.010 


3 


80.00 


55.87 


0.006 


0.200 


0.100 


0.010 



Figure 5.34 shows the angular position response for the three values of listed 

above. It can be seen that increasing the gain naturally causes the speed of response to 
increase as well. The time to reach the set point for the smallest gain, rj^= 0.01 rad / sec^ 

is approximately 50.0 seconds and half that, only 25.0 seconds using the largest, T]^= 

0.10 rad! sec^ . The fast response does come with a cost since the larger gain causes 
control action chatter once the steady state heading has been achieved, as seen in the lower 
trace of Figure 5.36, while the control action provided by the softer gain is relatively 
smooth. The limit cycle behavior is caused by the same reasons as explained for 
submergence control, namely the low damping and the sensitivity from the square root 
function in Eqn. (5.27). 
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Since the value of the saturation gain, <p^, in the first series of experiments was 

small giving a relatively sharp switching action, it was decided to increase this value in 
order to influence the amount of chattering in the steady state while maintaining a 
respectable response time. The parameters used for this series are in Table 5.8 and the 
results shown in Figures 5.37 through 5.39. 

Table 5.8 Parameters for Saturation Series 



Saturation 


h 


K 






% 


K 


1 


80.00 


55.87 


0.006 


1.000 


0.090 


0.100 


2 


80.00 


55.87 


0.006 


1.000 


0.090 


0.200 


3 


80.00 


55.87 


0.006 


1.000 


0.090 


0.400 


W/F 


80.00 


55.87 


0.006 


1.000 


0.090 


0.400 



It can be seen that the differences in saturation level have only a slight effect on the 
response time but does follow a trend of increasing switch sharpness (i.e. decreasing 0^) 

produces a faster system. Observing the control voltage in Figure 5.39, both the frequency 
and amplitude of the control voltage is decreased by increasing 0^. This is attributed to the 

fact that with a larger saturation gain, larger values of which reflect larger errors, 

will be attenuated by the sat function over a much larger range which decreases the control 
action in the steady state. 

Another approach was attempted to reduce the chattering by post filtering the 
control voltage commands. A first order digital filter of the form 

+ (I - ‘■■"'K- (5.28) 

was used. The value of the sampling time, T, was the usual 0.1 seconds and the time 

constant, t ,was 0.45 seconds. With filtering, the resulting control voltage is shown in the 
lower trace labeled " 0^ = 0.40 W / F" which still exhibits a limit cycle but is of smaller 

amplitude, and the frequency is greatly reduced. The results show that increasing the 
saturation to 0.40 rad ! sec and using a post filter provides an acceptable response time 
while significantly reducing the switch frequency and amplitude. 

To further reduce the actuator activity in the steady state, a dead zone on the 
command voltage was applied, that causes any voltage commands below a certain level to 
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be ignored and a value of zero sent to the thrusters. The following results show the 
comparison of a controller using only the post filter (Case 1) to one using the filter and a 
control dead zone of ±4.0 volts (Case 2). Table 5.9 gives the parameters used, and the 
response is shown in Figures 5.40 through 5.42. 

Table 5.9 Parameters for Dead Zone Series 





K 


K 


oc^ 




% 


K 


W/0 D.Z. 


80.00 


55.87 


0.010 


1.000 


0.090 


0.100 


With D.Z. 


80.00 


55.87 


0.010 


1.000 


0.180 


0.200 



Figure 5.41 shows a very rapid rise time for the controller using the dead zone 
since the switching gain, r\^ is relatively high but the control action is very small in the 

steady state as shown in Figure 5.42. It can also be seen that the control only activates for 
values above 4.0 volts as designed. Only one side of the dead zone is active since the 
ethemet tether was placing a small disturbance moment on the vehicle in the - y/ direction. 

3 . Heading Control Experimental Results (Command Generators) 

The command generators described in the previous chapter for submergence control 
can also be extended to include rotational motion. The two forms for heading control are 

[¥com^ WcoJ = <^( T „) (Fomi 1) 

or 

¥a,n.^ ¥coJ = G(¥o^ ¥f^ ¥ma.^ T T , ) (Fomi 2) 

where ij/q and Tq is the initial heading and starting time while y/f and Tj- is the final 

heading and time at the end of the maneuver. 

Form 1 of the command generator was used to control the vehicle from a heading of 
0 to n/2 radians as shown in Figures 5.43 through 5.45. The values for y/„^, and 

were 0.033 rad / sec and 0.006 rad / sec' and the nominal control gains and vehicle 
parameters were used. The vehicle tracks the commanded angular position perfectly but 
does not follow the desired rate very well during the constant angular velocity phase. This 
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is due to an uncompensated bias in the rate gyro but does not cause any performance 
degradation. 

The response for Form 2 of the command generator is presented in Figures 5.46 
through 7.49. This test controlled the vehicle from 0 to ;r radians in an elapsed time of 60 
seconds using a much gentler profile which has no region of constant angular velocity. 
Again the heading command is tracked very well even with the rate bias error. 

Both tests revealed the high thruster chatter that was seen from the submergence 
results and are due to the same reason. The errors are continually very small throughout the 
maneuver and the high gain effect of Eqn. (5.27) is again responsible. 

4. Conclusions From Heading Control 

The results of Sliding Mode heading control for the Phoenix using the lateral 
thrusters has been exceptional in both step response and command generator performance. 
The tendency of the lateral thrusters to chatter has not adversely affected the vehicle 
motions. Using post filtering and a dead band on the thruster input voltage lead to a 
significant reduction of this undesirable phenomena. The command generator performance 
was highly accurate, demonstrated by almost perfect tracking of the input profiles. 

D . LONGITUDINAL CONTROL 

Described in this section is the development of the sliding mode, longitudinal 
position control algorithms for the NFS Phoenix vehicle. The control has been 
demonstrated using wall servoing, which involves maneuvering the vehicle (along the 
body-fixed x:-axis) to a prescribed distance from one of the tank sides, using the STIOOO 
profiling sonar for position feedback. By using the sonar to ping against one of the tank 
walls, a smoothed range and range rate may be determined by proper filtering. Maneuvers 
of this type have important applications in the areas of inspection of underwater structures 
or close-up examination and classification of mines. The ability to servo a vehicle next to 
targets of interest greatly enhances mission capabilities. 

Results from a simulation study using this control technique, including current 
disturbances and thruster lags can be found in (Marco, 1992), while the analysis presented 
here details the experimental evaluation of the controller. 
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The first section outlines the development of the control algorithm followed by a 
discussion of a Kalman filter design for range rate estimation from the sonar, followed by 
experimental results for step response of wall standoff distance commands. Finally, some 
issues pertaining to sonar operation for this application are discussed. 

1 . Vehicle Model for Longitudinal Control 

The vehicle dynamics for longitudinal (x-axis) motion using the stem screws can 
be simplified to the following continuous time differential equation. 

M;m + M0|i(0| = 2 a,v,(/)|v^(r)| + 5f,{t) (5.29) 



where 

= m + 
and 

v,«)|v,(/)| = (v„( 0 |v,.«)| + v„( 0 |v „(/)|)/2 

m is the vehicle mass, and is the added mass of the body in the longitudinal direction. 
u(t) is the body-fixed rate for the longitudinal direction, is the square-law damping 
coefficient, and v,^(r), v^^(r) are the motor input voltages for the left/right stern screws 
respectively. The voltage to force coefficient is given by and Sf^{t) is the force error. It 
has an upper bound, 7 , such that Lj{5f^) < y where L is the L, norm of Sf^(t). Since 

motions are assumed to be restricted to the horizontal plane and along the longitudinal axis 
only, with no current, u{t) = x{t) and u(t) = x{t). 

Since the vehicle motion dynamics are expected to be second order in position, the 
sliding surface for servo control is specified as 

cr,(r) = kt) + A,x(0, (5.30) 
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where the tracking errors are defined as 



^(0 = - x(t) 

^(0 = Kom(f) - 

The sliding mode control law for servo control is given by 



(5.31) 



v^,(r) = ^j\y(t)\sgn(yit)) 

where 



(5.32) 



7(0 




Kon. + + 

V 




-^(0|a:(0I + rijanh{c7^(t) / (p^) 



SfA) ^ 

K J 



The test scenario is shown in Figure 5.49 where the range returned by the STIOOO sonar is 
denoted and since the direction towards the wall is positive x, motions in this 

direction will generate range values from the sonar which follow 



which implies that the rate is 
while for the vehicle 



R(t + At) < R(t) 
R{t + At) < 0, 
x(t + At) > 0. 



To deal with the sign difference between the range and vehicle rates, a change of variable 
may be introduced such that 



x{t) = Ro - ^(0 

m = - m 
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where is the initial range to the wall prior to the start of the maneuver and is also the 
position where x is defined to be 0. The commanded x position can then be described in 
terms of the wall standoff range by 



^com ^0 ^com ’ 



(5.33) 



which is used in the position error calculations forming the sliding surface, Eqn. (5.30). 

2 . Filter Design for Longitudinal Control 

The filter structure used for longitudinal control is identical to the one presented in 
the first part of this chapter for applied to vehicle submergence. In this case, the input 
signal is the range from the STIOOO profiling sonar. The filter provides a smoothed range 
and estimate of the range rate which will be used for vehicle control. The filter model is 
again based on a three state kinematics model for range excited by acceleration noise, 

k,{t) = R,{t) 

4(0 = 4(0 (5.34) 

4(0 = q{t). 

The states R,{t), 4(0. and 4(0 are estimates of the position, velocity, and acceleration 
of the range signal /?(r), while the measurement equation, with R{t) being the range, is 

R{t) = 4(r) + v(r), (5.35) 



where q{t) is taken to be the system noise and v(t) is the measurement noise. The gains 
used were also the ones obtained for Filter 2 defined earlier, namely 



L 



0.2544 
< 0.3727 > 
0.2731 



which provided good results for the environment of the test tank and nature of the signal. 
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Figure 5.50 shows the raw sonar data together with it’s filtered estimate and the 
vehicle velocity estimate. Range drop outs and false readings from the water column are 
evident in the raw signal as shown. Anomalies of this magnitude cause serious problems if 
used in a control law and must be ignored. One method to detect range anomalies is to 
monitor the residuals, r, given by 



which, for this filter, is the difference between the measured range and the expected range, 
and is known as the "innovations process" (Friedland, 1986). The covariance of the 
residuals, <7^, is the model based propagated state error covariance, given by 



If a residual exceeds 3(7^, the corresponding range return is declared an anomaly and is 
rejected. Since the measurement is invalid, the Kalman gain, L^, is set to zero and the new 

state estimate is propagated without correction. The condition for rejection can also be cast 
as a ratio given by 



which is commonly referred to as the "normalized innovation". 

Application of this technique provides satisfactory results, except for instances 
when multiple consecutive anomalies occur. In this case, filter estimates propagate without 
correction, and after some time, (depending on the estimated state), diverge to the point 
where filter lock is lost. Checks for filter divergence are incorporated into the vehicle 
operational software. The decision is keyed on the estimated range rate, where if 
> ^mwc divergence is suspected, and use of the filter for navigation is terminated. An 
3.0 ft /sec has been used, which is a maximum feasible velocity for the vehicle. 
Regaining lock is unfortunately not an easy task. Several options for filter re-initialization 
are available but depend on particular circumstances, and this an area for further work. 



r = Rk- hr,, 



(5.36) 



= + rq,_,r\ 



(5.37) 




(5.38) 



171 



3 . Longitudinal Control Experimental Results (Step Response) 



In this section, results obtained from in water testing of wall servoing control are 
presented. Each test began by submerging the vehicle to 2.5 ft using the depth control 
method outlined earlier to minimize surface effects during translation. The vehicle was also 
under heading control to maintain the longitudinal axis perpendicular to an end wall. To 
perform this maneuver, two phases are needed, one to identify the target, and another to 
servo the vehicle. The Prolog code used is : 



execute_phase(3) exec_find_sonar_target(X), repeat, phase_completed(3). 

phase_completed(3) exec_ask_sonar_target_found(X), X==l, 

exec_start_sonar_filter(X), asserta(complete(3)). 

phase_completed(3) ask_sonar_ping_out(X), X==l, exec_surface(X), 

repeat, ask_surface_reached(X), X==l, 
asserta(abort(3)). 



execute_phase(4) exec_next_setpt_data(X), exec_servo_X(X), 

exec_start_X_error_filter(X), exec_submerge(X), 
exec_rotate(X), exec_start_timer(X), repeat, 
phase_completed(4). 

phase_completed(4) exec_sleep(l,X),ask_X_reached(X), X==l, 

ask_depth_reached(X), X==l, ask_heading_reached(X), X==l, 
asserta(complete(4)). 

phase_completed(4) ask_time_out(X), X==l, exec_surface(X), 

printscCPHASE 4 ABORTED DUE TO TIME OUT!’), 
repeat, ask_surface_reached(X), X==l, 
asserta(abort(4)). 



Vehicle submergence is performed in phase 2, followed by activating the sonar to 
identify the distance to the wall. Note that there is no submerge or rotate command for 
phase 3, since the depth and heading commands for phase 2 are still in effect. Execution of 
the predicate "exec_find_sonar_target" starts the search by pinging against the end wall, 
and if 3 consecutive, consistent ranges are obtained, the predicate 
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"exec_ask_sonar_target_found" evaluates TRUE. At this time, "exec_start_sonar_filter" is 
executed and the Kalman filter algorithm, part the Execution Level, is initialized to the 
average of the three consecutive ranges, which completes phase 3. With the range to the 
wall determined, servoing control is activated in phase 4 where "ask_X_reached" succeeds 
if the vehicle reaches the set point within the allowed timeout. In phase 3, if the wall is not 
identified after 5 attempts, based on 3 consecutive pings, the predicate 
"ask_sonar_ping_out" is evaluated TRUE and the mission aborts. 

A nominal set of control gains and vehicle parameters were obtained through 
computer simulation and parameter identification of the wall servo motion, (Torsiello, 
1994), which are given in Table 5.10 below. 



Table 5.10 Nominal Parameters and Gains for Servoing Control 



Parameter/ 

Gain 


M, 


k 




A. 




<l>. 


Unit 


2 

lb - sec 

fi 


2 

lb — sec 


lb 


rad 

sec 


ft 

sec^ 


jl 

sec 


Value 


14.86 


1.33 


0.025 


0.2 


0.1 


0.2 



Two test runs using the above values are shown in Figures 5.51 through 5.53. A 
commanded standoff distance, of 3.0 feet was chosen, with an initial distance 

from the wall approximately 9.3 and 11.0 feet for Tests 1 and 2 respectively, with 
^am — ^am ~ Both rcsults show an extremely well behaved position response 

exhibiting no overshoot, regardless of the initial starting point. Figure 5.52 reveals that the 
estimated velocity is slightly noisy but does not adversely affect the results. Shown in 
Figure 5.53 is the voltage commands to the stern screws, which exhibit some chattering 
due to the controller switching term, but is not at such a high frequency to cause significant 
actuator wear. 

4 . Robustness Analysis of Longitudinal Motion Maneuvers 

A robustness analysis using the control design and experimental data from above 
will now be presented. Eqn. (2.33), cast in terms of the longitudinal control parameters can 
be written as 
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(5.39) 






r F 

i X X 

M. 



M. 



■|(^ - rj| + - })\](Ko. + Ai) 



where 



- ^ TT ^ ^^x^^ ^^x > F — < -^ < y/, y, > ], 



M, 



Oi. 



and |/, - = F^, where = -b^x\x\. 



If the experimental data is substituted into (5.41) with appropriate values for the 
parameter uncertainties, a time history of the minimum value of required for stability 

can be produced. Figure 5.54 shows the effect different levels of uncertainty listed in Table 
5. 1 1 have on the required magnitude of applied to the data from Test 1 . 



Table 5.11 Uncertainty Levels for Longitudinal Control 
Robustness Analysis 



Uncertainty Level 


F 


7 


F 


1 


0.0 


1.2 


1.2 


2 


0.0 


1.5 


1.5 


3 


0.0 


2.0 


2.0 



Recalling that x = -x for step inputs, the shape of the curves are proportional to the 
vehicle velocity, while the magnitude is scaled by the uncertainty level. During the steady- 
state phase of the maneuver, the requirement on rj^ shrinks since the velocity is very small. 
Since the controller for the experiment used a value of 0.1 for 77 ^, and provided a stable 
response, it can be concluded that the parameter estimates used in Table 5.10, are close to 
the actual values. 

5 . Sliding Mode Verses PD Control and Sonar Input Power 
Considerations 

The results presented so far have used a Sliding Mode controller which has 
provided extremely precise positioning response for the non-linear system involved. 
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However, previous work used a simple Proportional Derivative (PD) linear controller for 
the same task. While PD control is simple to implement and executes rapidly in real time, as 
compared to the model based sliding mode control, the performance is inferior as shown in 
Figure 5.55. A large overshoot results using the PD controller for the set point = 3.0 

feet, while the SMC shows none, clearly demonstrating the superiority of the non-linear 
controller for this application. 

During the course of the wall servoing experimental work, it was discovered that 
the sonar power level must be chosen carefully when used in the test tank. The nominal 
power level was determined by placing the sonar head in the middle of the tank and 
adjusting it until the most favorable results were obtained. A level of 12% of full power 
was found to be appropriate for the tank environment, and turned out to be suitable for 
ranges as close as 4.0 feet from the wall, but not closer. By placing the head closer to the 
wall, over ensonification occurs and the range signals become extremely erratic as shown 
in Figure 5.56. With the head some distance from the wall, the signal is very clean and 
stable, but after settling to approximately 3.0 feet away, the signal begins to breakup, 
causing the filtered range to diverge. By lowering the power setting to 5% corrected the 
problem for "close to wall" servoing, and was sufficient for target detection as far away as 
12.0 feet. One method to automatically adapt the power setting is to use a formulation such 
as 



Power =f(R), (5.40) 

which automatically reduces the sonar power as the head nears an object. Although this 
approach is encouraging, it has not been incorporated into the sonar control software, but is 
an area for future work, where learning algorithms can be applied. 

6. Conclusions From Longitudinal Control 

The results of sliding mode wall servoing control have shown exceptional accuracy 
in longitudinal positioning of the vehicle. Using a Kalman filter modified to reject range 
outliers, provided very accurate and stable signal conditioning from the ST 1000 sonar data. 
A comparison between sliding mode and proportional derivative control was presented 
which demonstrated the superiority of the non-linear approach. Future work in this area 
could include the use of two sonars to enable both longitudinal and lateral positioning of the 
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vehicle for more general control scenarios. Also, extensions to behaviors used in wave 
surge and current conditions using this approach could be done. 

E. COORDINATED SUBMERGENCE/HEADING CONTROL USING 
COMMAND GENERATORS 

1 . Command Tracking Performance 

In some robot motions, tracking to commands for multiple behaviors operating 
simultaneously becomes important. For instance, while maneuvering around targets, 
position and orientation of the vehicle may need to be coordinated. Coordination of control 
behaviors requires the use of time synchronized commands through command generators. 
To prove that these behaviors are possible in an underwater environment, experiments were 
conducted to simultaneously submerge and rotate the vehicle to a predetermined depth of 
3.0 feet and a heading of 180° respectively. It was specified that the final depth and 
heading both be reached at 60 seconds from the beginning of the maneuver, and was 
accomplished for both control modes using command generators. 

A section of the Prolog code used to control the mission appears below, and 
executes until the pre-defined depth and heading has been attained, which is determined by 
the error criteria presented in Chapter IB. 



execute_phase(2) 

exec_submerge(X), X==l, 
exec_rotate(X), X==l, 
exec_start_timer(X), 
repeat, phase_completed(2). 

phase_completed(2) ask_depth_reached(X), X==l, 
ask_heading_reached(X), X==l, 
asserta(complete(2)). 

phase_completed(2) ask_time_out(X), X==l, 
exec_surface(X), repeat, 
ask_surface_reached(X), X==l, 
asserta(abort(2)). 

phase_completed(2) ask_sys_problem(X), X==l, 
exec_surface(X), repeat. 
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ask_surf_reached(X), X==l, 
asserta(abort(2)). 



It should be noted that the Prolog code is identical to that used for simultaneous depth and 
heading control for step command inputs. The difference lies in the Tactical Level mission 
file, which specified the use of command generators for both depth and heading control. 
Refer to Chapter HI for a full description of the Tactical Level mission input file. 

The following tables give the values used in the vehicle control laws developed 
earlier for submergence and heading control as applied to the coordinated maneuver 
experiments. 



Table 5 . 1 2 . Parameters for 

Submergence Control 



Table 5.13. Parameters for Heading 
Control 



Parameter 


Value 


Unit 


m 


13.5 


lb 




13.5 


lb 


K 


28.8 


lb/ ft- sec^ 




0.004 


Ib/V^ 


A, 


0.400 


rad / sec 


A, 


0.040 


rad / sec^ 


T] 


0.1 


ft / sec^ 




0.2 


ft / sec 



Parameter 


Value 


Unit 


L 


40.0 


lb — ft — sec^ 


L 


40.0 


lb- ft - sec' 


K 


55.87 


lb- ft- sec^ 




0.006 


Ib-ftlV^ 


A 


0.200 


rad /sec 


V 


0.200 


rad / sec^ 


0 


0.200 


rad / sec 



Figures 5.57 and 5.58 show the normalized time responses for depth/depth rate and 
heading/heading rate respectively. Although the depth command is accurately tracked 
during the transient phase, an overshoot occurs near the final set point, caused buy the 
vehicle becoming "heavy" from hull compression. At this point, integral action from the 
controller quickly compensates for the error. Since no disturbance force was present in 
rotation, the heading response shows a very precise tracking performance with virtually no 
error. 

Referring to the depth rate response, a very noisy signal is evident, caused by 
discretization noise from the depth cell A/D converter. Although the signal is far from 
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clean, the overall tracking performance is not adversely affected. The heading rate 
measured from the onboard gyroscope shows a definite tracking error, and is attributed to a 
non-zero bias in the unit. Figure 5.59 shows that the depth and heading are simultaneously 
controlled to the command generator specification except for the small depth overshoot at 
the end of the maneuver. 

2. Conclusions From Coordinated Control 

It has been shown that is a relatively easy task to program coordinated vehicle 
maneuvers since the generality of the Strategic Level rules allow the method of control to be 
determined by the Tactical Level. Accurate, simultaneous tracking of the command 
generator trajectories proved that the servo level controllers are currently very well tuned 
for the vehicle maneuvers shown. 

F. CONCLUSIONS 

The results presented have shown that the Phoenix can be precisely controlled in the 
test tank environment using thrusters. Development of sliding mode controllers for 
submergence, heading, and longitudinal control have proven to be robust and have shown 
to provide exceptional vehicle performance. The flexibility of the Strategic Level rules 
allowed many different control scenarios to be quickly tested and evaluated without major 
code modifications. Using command generators provided extremely precise time based 
maneuvering and proved very effective when activated concurrently for multiple control 
modes. 
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G. CHAPTER V FIGURES 
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Filter Gains 




Figure 5. 1 Steady State Kalman Filter Gains vs. Time for Filter 2, 
where L„ = [1(7) 1(2) L{3)Y . 
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Depth (Feet) 




Figure 5.2 Unfiltered Depth Signal vs. Time. 
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Depth Rate (Feet/Sec) 





Figure 5.3 Depth and Depth Rate vs. Time Response Using Filter 1. 
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Depth Rate (Feet/Sec) 





Figure 5.4 Depth and Depth Rate vs. Time Response Using Filter 2. 



Depth Rate (Feet/Sec) 





Figure 5.5 Depth and Depth Rate vs. Time Response Using Filter 3. 
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Figure 5.6 Magnitude of Velocity Estimate vs. Frequency for Filters 1 , 2, and 3 




Figure 5.7 Phase Angle of Velocity Estimate vs. Frequency for Filters 1, 2, and 3. 
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Depth (Feet) 




Figure 5.8 Simulated Depth Response vs. Time Using Filters 1, 2, and 3. 




Figure 5.9 Simulated Depth Rate Response vs. Time Using Filters 1, 2, and 3. 
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Control Voltage (Volts) 




Figure 5.10 Simulated Depth Acceleration Response vs. Time Using Filters 1, 2, and 3. 
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Figure 5. 1 1 Curves for Linear and Non-Linear Formulas of (p . 



191 




Figure 5.12 Switching Curve for Various Formulas of 0. 
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Depth (Feet) 




Figure 5.13 Simulated Depth vs. Time Response for a Constant and Variable. 
Boundary Layer. 
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Depth (Feet) 




Figure 5.14 Experimental Depth Response vs. Time for Different Values of 
Vertical Damping, b,. 
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Control Voltage (Volts) 
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Figure 5.15 Experimental Vertical Thruster Control Voltage vs. Time for Different 
Values of Vertical Damping, b^. 
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Figure 5.16 Experimental Vertical Thruster Control Voltage vs. Time for Different 
Values of Vertical Damping, b,. 
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Depth (Feet) 




Figure 5. 17 Experimental Depth Response vs. Time for Different Values of 
Thruster Effectiveness, a,. 
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Control Voltage (Volts) Control Voltage (Volts) Control Voltage (Volts) 




Figure 5. 1 8 Experimental Vertical Thruster Control Voltage vs. Time for Different 
Values of Thruster Effectiveness, a... 
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Depth (Feet) 




Figure 5.19 Experimental Depth Response vs. Time for Different Values of 
Switching Gain, rj. 
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Control Voltage (Volts) Control Voltage (Volts) Control Voltage (Volt 




Figure 5.20 Experimental Vertical Thruster Control Voltage vs. Time for Different 
Values of Switching Gain, vj . 
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Depth (Feet) 




Figure 5.21 Experimental Depth Response vs. Time for Sliding Surface Position 
Error Coefficient, X . 
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Control Voltage (Volts) Control Voltage (Volts) Control Voltage (Volts) 




Figure 5.22 Experimental Vertical Thruster Control Voltage vs. Time for Sliding 
Surface Position Error Coefficient, A . 
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Depth (Feet) 




Figure 5.23 Simulated Depth Response vs. Time for Light, Heavy and Neutrally 
Buoyant Vehicle. 
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Depth (Feet) 




Figure 5.24 Simulated Depth Response vs. Time for Integral Control starting at 
t = 0.0 Seconds. 
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Figure 5.25 Simulated Depth Response vs. Time for Integral Control Starting at a 
Preset Time of 40.0 Seconds. 
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Figure 5.26 Experimental Depth Response vs. Time Using Integral Control. 



206 




Figure 5.27 Experimental Vertical Thruster Control Voltage vs. Time Using 
Integral Control. 
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Figure 5.28 Depth Response vs. Time Using Command Generator Design 1. 
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Figure 5.29 Depth Rate Response vs. Time Using Command Generator Design 1. 
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Figure 5.30 Vertical Thruster Control Voltage vs. Time Using Command 
Generator Design 1 . 
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Depth (Feet) 




Figure 5.3 1 Depth Response vs. Time Using Command Generator Design 2. 



Depth Rate (Feet/Sec) 




Figure 5.32 Depth Rate Response vs. Time Using Command Generator Design 2. 




Figure 5.33 Vertical Thruster Control Voltage vs. Time Using Command 
Generator Design 2. 
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Heading Angle (Radians) 




Figure 5.34 Heading Angle vs. Time Step Response for Switching Gain, T]^. 
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Heading Rate (Rad/Sec) 






Figure 5.35 Heading Rate Response vs. Time for Switching Gain, 
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Figure 5.36 Lateral Thruster Control Voltage vs. Time for Switching Gain, 
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Heading Angle (Radians) 
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Figure 5.37 Heading Angle Step Response vs. Time for Switch Saturation Level, 0 ^. 



Heading Rate (Rad/Sec) 




Figure 5.38 Heading Rale Response vs. Time for Switch Saturation Level, 
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Motor Voltage (Volts) 
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Figure 5.39 Lateral Thruster Control Voltage vs. Time for Switch Saturation Level, 0^. 
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Heading Angle (Radians) 




Figure 5.40 Heading Angle Step Response vs. Time With and Without Control 
Voltage Dead Zone. 
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Figure 5.41 Heading Rate Response vs. Time With and Without Control 
Voltage Dead Zone. 
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Figure 5.42 Lateral Thruster Control Voltage vs. Time With and Without Control 
Voltage Dead Zone. 
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Heading Angle (Radians) 




Figure 5.43 Heading Angle Response Using Command Generator Design 1. 
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Heading Angular Hate (Had/bec) 




Figure 5.44 Heading Rate Response Using Command Generator Design 1. 
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Figure 5.45 Lateral Thruster Control Voltage Using Command Generator Design 1. 
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Heading Angle (Radians) 




Figure 5.46 Heading Angle Response vs. Time Using Command Generator Design 2. 
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Figure 5.47 Heading Rate Response vs. Time Using Command Generator Design 2. 
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Figure 5.48 Lateral Thru.ster Control Voltage vs. Time Using Command 
Generator Design 2. 
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Figure 5.49 Wall Servoing Experimental Setup. 



229 




Figure 5.50 Filtered and Unfiltered Range. 



Sonar Range (Feet) 




Figure 5.51 Position Response vs. Time for Tests 1 and 2. 
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Figure 5.52 Position Rate Response vs. Time for Tests 1 and 2. 
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Figure 5.53 Stern Screw Control Voltage vs. Time for Tests 1 and 2. 
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Figure 5.54 Effect of Parameter Uncertainty on Gain 
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Figure 5.55 Sliding Mode Verses PD Controller Position Response. 




Figure 5.56 Sonar Ensonification in the NFS Test Tank. 
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Nomalized Depth / Depth Rate 




Figure 5.57 Normalized Depth and Depth Rate Response vs. Time of the 
Coordinated Maneuver. 
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Figure 5.58 Normalized Heading and Heading Rate Response vs. Time of the 
Coordinated Maneuver. 
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Nomalized Depth / Depth Rate 




Figure 5.59 Normalized Depth vs. Heading Response of the Coordinated Maneuver. 
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Figure 5.58 Normalized Heading and Heading Rate Response vs. Time of the 
Coordinated Maneuver. 
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Nomalized Depth / Depth Rate 




Nomaiized Heading / Heading Rate 



Figure 5.59 Normalized Depth vs. Heading Response of the Coordinated Maneuver. 
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VI. LOCAL AREA NAVIGATION 



While there has always been a need to determine the global position of an 
underwater vehicle, in some missions involving search, mapping, and intervention with 
objects, navigation to local area landmarks is more appropriate and precise. In minefield 
reconnaissance operations, there is also a need to determine position relative to a target for 
the purpose of taking video pictures, viewing from different aspects, and even charge 
placement. If the object lies in an unstructured environment, the vehicle must use active 
sensors to perform these operations. Once maneuvering control around objects in the local 
area scene is understood to a satisfactory degree, intervention using manipulators and other 
tactile devices will be enabled. Such activities as changing out a battery pack for a bottom 
mounted sensor or finding and entering an underwater garage for re-powering will then 
become commonplace. 

This chapter concerns the analysis of local area maneuvering using sonar based 
feature detection from the local scene. As this process takes a significant time to complete, a 
mathematical model of the vehicle response is used to provide control inputs during periods 
when sonar updates are not available. In the class of vehicles designed for the intervention 
mission, (Marks, et. al., 1994) have studied the problem of servo positioning the OTTER 
vehicle to visual cues from stereoscopic cameras, although monocular video data was used 
to perform edge detection and servo control of the pan and tilt mounting coupled to the 
vehicle platform. Smith, et al. have proposed the use of an acoustic single- 
transmitter/multiple receiver design for local area navigation, although preliminary data 
from the sonars alone seem encouraging, it has yet to implemented in an actual vehicle. 

Section A. covers the formulation of the three degree-of-freedom equations of 
motion for the vehicle (longitudinal, lateral, and heading), which will be used as part of the 
a model based navigation control. The next section address the algorithms used to locate 
and track a navigation target with the STIOOO sonar, and is followed by a description of the 
control methodology which incorporates model based control with position updates from 
the sonar. Simulation and experimental results of the control performance are given, and 
the chapter concludes with a discussion of an improved target tracking technique. 



241 



A . MODEL BASED CONTROL FORMULATION 

Absent of an inertial position reference, and where sonar position data arrive 
asynchronously, and infrequent, a dynamic model of the vehicle is used for state 
information between updates. A three degree-of-freedom model (longitudinal, lateral, and 
heading) is used since the motion for this experiment is restricted to the horizontal plane 
with the depth maintained by a separate controller. The model is obtained by including 
drag, added mass, and steady state thrust, and for surge is 

M^u{t) + b^u{t)\u(t)\ = 2a^v^(/)|v^(/)| (6.1) 

The sway directional equation of motion is 

M,.v(r) + &,v(/)|v(/)l = a,,v^„(/)|v^„(r)| + avV,„(/)|v,„(r)| (6.2) 



and finally the equation for the yaw dynamics becomes 

/, r(0(t) + b^r(t)\r(t)\ = a^v,„(/)|v,„(/)| - a^v,„(r)|v„(/)| (6.3) 



where 

= m + M^. = m + m,,., /, = /^ + 



and 



v.(0|v,(0| = (v„(/)|v„(0| + v„(,)|v„(0|)/2. (6.4) 

m is the vehicle mass, the mass moment of inertia about the body-fixed z-axis, and 
the subscript "a" refers to the added mass or inertia of the body. u{t), v(r), and r{t) are 
the body-fixed relative velocities for longitudinal (x-axis), lateral (y-axis), and heading 
( i//) directions, b^, b^., b^ are the square-law damping coefficients, v,,(/), v^^(/), and 

v^,,(r), v^,,(/) are the thruster motor input voltages for the left/right stem screws, and the 

bow/stem lateral thrusters respectively. The voltage to force/moment coefficients are given 
by a^, (Xj , and a^. 
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The above dynamic equations can be formulated using matrix notation as 

Mx{t) = f{x{t),b) + g{a)u{t) (6.5) 

and vehicle kinematics are defined by 

i{t) = h{y/)x{t) + u^{t). (6.6) 

The body-fixed relative velocities are 

x{t) = { u{t) v{t) r{t) y, (6.7) 

and the global position and orientation is given by 

z(t) = {X(0 Y(t) noV- (6.8) 



The vector describing the hydrodynamic drag that is a function of the relative velocity and 
square-law damping coefficients, b = [b^ } is 

f(xit),b) = {-Zj^m(/)|m(/)| - 6,.v(r)|v(r)| - b^rit)\r{t)\Y , (6.9) 



the mass matrix is 



M = 



M, 0 0 

0 My 0 
0 0 1 . 



Zj 



( 6 . 10 ) 
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and input gain matrix, which is solely a function of the thruster coefficients, 
a ={ a, } , is 



g(a) = 



2a, 

0 

0 



0 0 
a, a, . 



( 6 . 11 ) 



Finally, the control input vector is defined as 



«(') = {'',«)|v,(0| v„,(/)|v„,(0| v,„(^)|v.„(/)|}^ (6.12) 



For the case of translation in X , F and rotation y / , the transformation matrix from 
the body-fixed axes to the global reference is given by 



hiwiO) = 



cos{ lf/{t)) 
sin( y/(t)) 
0 



-sin{y/{t)) 
cos( y/(t)) 
0 



0 

0 , 
7 



(6.13) 



and it's time derivative is 



hi y/it), y/(0) 



-y/(/)sin(y/(f)) -y/(t)cos(y/(f)) 0 
y/(t)cos(y/(t)) -y/(f)sin(y/(0) 0 

0 0 0 



(6.14) 



Any current disturbances are represented by 

«c(0 = {m.,( 0 «,,(/) or (6.15) 
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The sliding mode control law can now be formulated defining the tracking error 
vector in terms of global coordinates as 



z(t) 








'z(t) 


z(t)_ 




_^com 




_z(t)_ 



( 6 . 16 ) 



Now that the tracking error has been formulated, an equation defining the sliding 
surface in terms of this error can be written as 



<T(z(r)) = [5; 




( 6 . 17 ) 



where 



a{x{t), z(0) € ; S„ S, e 91^^^ 

The elements of Sj and S 2 can be selected to provide the desired performance of the closed 
loop system. For the case of planar control, and choosing Sy as the identity ,they are 





'} 


0 


o' 






0 


0 ‘ 


Si = 


0 


1 


0 


S 2 = 


0 


K 


0 




_0 


0 


1 _ 




0 


0 





If Sy is identity, and u^.(0 is assumed zero, the sliding mode control becomes 

U;(0 = g(»)-'(Mh-%.J0 - /(•)) 

"2(0 = g (» y ' ( 6 . 18 ) 

Uj(t) = g{»y'Mh'\») ri(a{*),<t>) 
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where 



0 = {t 0v <l>y, Y and a = {g, ct, 



In Eqn. 6.18, the switching term 77(cr(*),0) is more clearly defined as a 3 by 1 column 
vector where each element contains the appropriate individual response mode switching 
function, so that 






ri^satiGj (t>J 
T],sat{G^. / 
n^sat{G^ / 0^) 



(6.19) 



and u,{t), U 2 (t), and u,(t) contain the acceleration, velocity and switching terms 
respectively. The inverse of the input gain matrix for this case is found directly as 







0 


0 


g{a)-‘ = 


0 


pa. 






0 


pa, 


1 

<N 

1 



Since the motion is restricted to the horizontal plane. 






cos(y/(t)) sin(y/(t)) 0 
-sin(y/(t)) cos(y/(t)) 0 
0 0 1 



( 6 . 20 ) 



( 6 . 21 ) 
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The control vectors in terms of the individual parameters and gains are 



M ;(0 = 

(;/2aj(M,co5V/(r)X,„„(r) + M^sinii/{t)Y^^Jt) + 4«(0|m(01) 

(7/2av)(Mv( - sin\i/{t)X^„Jt) + cosy/{t)Y^„Jt)) + &,,v(/)|v(/)|) 

< + + b^r{t)\r(t)\) 

- sin\i/{t)X^,,Jt) + cosy/{t)Y^,,J0) + 4.v(/)|v(/)|) 

i - (//2aJ(Tv/,„„(r) + ^^r(r)[r(/)|) 

« 2(0 = 

- sini}/{t)X(t) + cosy/(t)Y{tfj + ^X^cos\i/{t)X{t) + X^.sin\i/{t)Y{t)^ 
y/{-cos\i/{t)X{t) - siny/{t)Y{t)^ - X^siny/it)X(t) + A,co5i//(r)f(oj 

(7/2d,.)M,.^ y/{-cosy/{t)X(t) - siny/{t)Y{t)^ - X^siny/it)X{t) + X^.cosy/it)Y(t)^ 
- (]/2a^)l,X^if(t) 
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Ujit) = 



[ll2a^)M^(r\^cos\i/{t)sat[oJ(p^) + rj,5mi//(0^flr(cT,/‘^.v) ) 

(7/2a,,)M,( -r\^sm\i/{t)sat{Gj<p^) + %cos\i/{t)sat[G^I(p^^ 

(7/2ajM,( -ri^siny/(t)sat{Gj<p^) + Tj^cosy/(t)sat(G^./(l>^)) 

- [l/2a^)l^ri^sat[G^/(()^) 

The control voltages to each actuator is given by 

v .(0 = ^|\yA0\s8niy,(t)) 

'^hi,(o = ^\yhiM^s"(yH,(0) 
v.,/,(o = ^J\y.i,(o\^8niy,„{t)) 

where 

7x(0 = «/ + “^ + 

' ^ A 7.7) n7.7) AU) 

7w,(7) = «) + M-> + Ml 

' '( 2 . 1 ) n 21 ) - 121 ) 

7v/i(0 = M, + «, + M, 

• A.t.7) ^.i.7) *\j.7) 

Now that the dynamic model of the vehicle has been established, the next step 
required to perform local area navigation is the selection of a suitable target for reference. 
This topic will be covered in the next section. 



( 6 . 22 ) 

(6.23) 

(6.24) 
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B . TARGET DETECTION WITH SONAR 



To perform local area navigation using sonar, it is necessary to select an easily 
discernible feature in the vehicle operating area and use it as a fixed reference. To enable 
repeatable and unambiguous detection of the reference feature, the target feature should be 
stationary and reasonably unique with respect to other structures in the sonar field of view. 
Also, to classify these features, each must be segmented into a separate object and analyzed 
to see if it posses the structural properties of the desired target for reference. 

For the results presented, the target used for the local navigation reference was a 
1.0 foot diameter, 2.5 foot long cylinder placed vertically in the water column of the NFS 
test tank. The Tritech STIOOO profiling sonar head was used mounted vertically in the nose 
of the Phoenix vehicle. The head uses a stepper motor which can mechanically rotate the 
transducer through 360<^ with respect to it's mounting at a minimum angular resolution of 
0.9®. For each step, the sonar is pinged and a single range value is returned which enables 
a complete profile of the area surrounding the vehicle to be constructed. 

An actual scan of the cylindrical target and square tank walls is shown in Figure 
6.1. A sweep width of ±35® and angular resolution of 1.8® was used. Each dot or "pixel" 
corresponds to a discrete range value returned by the sonar for a given angular position of 
the transducer head. Several disjoint groups or segments of pixels are visible in the field of 
view: the two sections of the tank wall, and the cylinder which casts an acoustic shadow 
against the wall. Since sonar range drop outs and noise are common with sonars, the tank 
wall to the upper right of the cylinder is broken up into several segments, although in 
reality, it is a continuous feature. It is this nature of acoustic sensors that lead to the 
development of the following algorithms for cylinder detection in the NFS test tank. 

Since the cylinder has been chosen as the desired target for the local area reference, 
returns from the tank walls will be filtered out and ignored. Separating a cylinder from a 
wall can be accomplished by segmenting each contiguous, disjoint group of range pixels 
and analyzing them for the desired characteristics of a cylinder. The basic method to isolate 
these segments is outlined in the flow diagram in Figure 6.2. The filter algorithm is 
initialized by pinging several times at a fixed bearing to obtain an average range value, r . 
The head is then commanded to scan in a clockwise direction and each range return is first 
tested for feasibility. If the range is zero or if it exceeds the maximum operating range, 
r^, it is ignored and that range, r,-, is set to the current average range, r, and the scan 
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proceeds. If the range is feasible, a test is performed to see if it lies within an error band of 
± Ar of the average and if so, the value of r is recalculated using the new range. The 
range and the associated bearing angle is then stored in a vector of size n, (the number of 
pixels defining the segment). If the range falls outside of the error band, a flag is set to 
examine how closely subsequent returns compare to the new range. A secondary average, 
is initialized to this value and a new segment is declared if the next adjacent 
ranges are consistent with this average at which time the current average is set to The 
old segment is now terminated at i - and the range, bearing and pixel count values are 

processed to extract any shape information they may provide. If the subsequent ranges, 
less than pixels are inconsistent with and fall near the previous average, a new 

segment is not assumed and the scan continues using r . These "false alarms" occur quite 
frequently due to the nature of the sonar returns which contain drop outs and false ranges. 
The value of can be varied depending on the environment of operation. For the test 
tank which provides relatively clean signals, the value of is typically 3, but in more 
noisy conditions, a larger value should be used to provide higher filtering. 

Once a separate segment has been identified, the vector containing it's ranges and 
bearing angles is analyzed. The flow diagram for this algorithm is shown in Figure 6.3. To 
determine if the object defined by the segment is a cylinder, it must posses the following 
characteristics: 

1. Consist of a sufficient number of pixels, n, that does not exceed a 
maximum, If the number of pixels is large, in this case greater than 10 it 

can be safely assumed the segment is a wall due the relative size of the cylinder. 

2. Be in front of the tank walls. This is an obvious observation since the 
cylinder is assumed to be in the tank but must be included in the algorithm to 
avoid confusion by perceived cylindrical shaped areas on the wall due to noise. 

3. Have a central range closer than it's edges. Since a cylinder appears the same 
from any direction in a horizontal plane, the center of the segment will always 
be closer the sonar than the beginning and ending edges. 

The preceding algorithms have been used with much success in the NFS test tank 
and should operate well in an open water environment especially since the tank walls will 
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be absent and the reference target the most visible object in the area. This procedure can be 
modified to search for other geometric shapes since the idea of segmentation of each feature 
is retained but does not attempt to supplant more sophisticated and robust pattern 
recognition algorithms available. Additionally, this method was adopted since it can be 
executed in real time and is simply used as a means to perform the tasks described in the 
following sections. 

C. RELATIVE POSITION ESTIMATION 



Once the reference target has been identified, it becomes the origin of the navigation 
coordinate frame where the X -axis is aligned with heading 0 degrees and the Y -axis along 
a heading of 90° as shown in Figure 6.4. The two dimensional position vector to the origin 
of the vehicle body-fixed reference with respect to the navigation frame at detection time T 
is 



RJT) = 



\XJT)] 



-(rJT) + RJT)) 



where 



and jc^, is the position of the sonar head in vehicle coordinates. 

+ r^.yi)cos{ii/(T) -t- I/A, (7))! 
RcyAT) = + ;-^jsin(i//(T) + I/A,,(r))| 



(6.25) 



(6.26) 



(6.27) 



where R^yiiT) is the sonar range to the target, i/a, (7) is the heading angle of the sonar 
beam, and for the case of a cylindrical target, is it's radius. After the target and the 

location of the vehicle is found, the delay time between re-acquisitions is reduced by 
commanding the sonar to sweep across the target at a prescribed minimum sweep angle 
y/y^, about a heading to the center of the target. 
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D . POSITION UPDATE 



Because of the physical speed limits to mechanical scanning, there is a delay time of 
up to 10 seconds between successive detections of the target, the navigational scheme 
proposed employs the dynamic model between position updates. Eqn. (6.5) is integrated to 
obtain estimates of the vehicle position denoted X{t), and y(r) during this time. The scan 
direction command angle between position updates is computed using 



f 

Wsj(0= atan2 

V 



-( Y{t) + x^sinjy/(t)) + y,co5( i//-(0) 
-(X(r) + x^cos(\(r{t)) + y^sin(y/it)} )j 



- W(0 



(6.28) 



and a maneuver using this approach is shown in Figure 6.5. In circumstances where the 
scan width is too narrow and there exists a large discrepancy between the model and the 
actual vehicle, the scan direction calculated from the estimates of position can be in error. In 
these cases, and if the target has not been reaquired within a specified time, the head is then 
commanded to return to continuous sweep re-acquisition mode. One approach to reduce 
this possibility would be to increase the scan width, to say 120 degrees but this would 

increase the time between updates and has not been implemented. 

For vehicle control in a plane, the complete state is defined by 

X(t) = { u(t) v(t) r(t) X(t) Y(t) y/(t) (6.29) 



and the block diagram representation of the control scheme is shown in Figure 6.6. When 
the cylinder has been identified, the model is asynchronously updated at time of target 
detection using a Kalman filter of the form 



X(T) = (1 - K)X~(t) + KX^XT) 
Y(T) = (] - K)Y-(t) + KY^XT) 



(6.30) 
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where 



K = - V - ■ , (6.31) 

and cr^ is the variance of the system model estimate of position and o] is the variance of 
vehicle position using the sonar. X~{t), and Y~{t) is the current estimate of position from 
the model just before the correction from the sonar is obtained. This analysis assumes the 
position estimate from the sonar is extremely accurate and the model very inaccurate. 
Therefore, the variance for position from sonar is set to 0 and infinity for the model. This 
causes the current estimate from the model to be disregard at the time of sonar update and 
reduces Eqn. (6.30) to 



X(T) = XJT) 



Y(T) = YJT) 



(6.32) 



which states complete confidence in the sonar. At this time the dynamic model state is reset 
to the values obtained from Eqn. (6.32). 

The word "Kalman" is somewhat loosely used here as the model error variance, cl 
is not predicted and propagated along with the positional estimates. However, the fusion 
gain, K, is computed on the basis of a minimum fusion error variance for Gaussian errors 
assuming that the error statistics are known a priori. 

The onboard gyroscopes provide the heading angle and yaw rate values at 10 Hz, 
which are synchronous and highly accurate and no estimation of these is required. The 
observation vector is defined by 



y(r) = CX(t) 



(6.33) 



where the observation matrix is 
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'0 0 0 0 0 0 ' 

0 0 0 0 0 0 

0 0 1 0 0 0 

c = 

0 0 0 0 0 0 

0 0 0 0 0 0 

0 0 0 0 0 1 

With only these two measurements made, Eqn. (6.33) reduces to 




(6.34) 



which is used each time step in the vehicle controller and dynamic model along with the set 
point vector 



r(t) = 



KJO 

Y.;JO 

[WaJO 



E. SONAR ENVIRONMENT SIMULATION 



(6.35) 



Before in water testing was performed, simulation of the effectiveness of the local 
area navigation algorithms were studied by simulation. Since the purpose of this work is to 
interact with the underwater environment using the sonar, the existing dynamic model of 
the vehicle alone would not be sufficient to test and evaluate these algorithms. The dynamic 
model is simulated by control inputs to the actuators only and has no knowledge of an 
external environment such as submerged obstacles, the bottom, or even other vehicles in 
the immediate area. This prompted the creation of a simple but effective simulator which 
modeled the physical environment in the computer using a collection of objects made up of 
three-dimensional rectangular polygons. Coupling the geometric representation of the sonar 
head motions and the dynamic model of the vehicle, enabled the navigation algorithms to be 
quickly evaluated and modified before in water testing was performed, and was found to be 
an essential development tool. 
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The underwater environment was built from 3-D rectangular polygons which means 
there exists for each vertex an ordered triple (x, y, z) in a global coordinate system. The 
polygons which define the NPS test tank with cylindrical target is shown in Figure 6.7 and 
are labeled 0 through 18. The sides of the cylinder not shown are unlabeled for clarity. This 
gives a very good representation of the operating environment for the actual tests to be 
done. Once the physical shape of the environment was defined, detection of the objects is 
performed using a polygon intersection technique described in detail in Appendix H. The 
line that intersects the polygon represents the sonar beam, and the origin of the beam is 
P{0) and the current direction the beam is pointed is described by unit vector e. Solution 
of Eqn. (H.7), which is the point of intersection of the sonar beam is the range to that 
particular plane. This operation is repeated for each plane describing the environment to 
determine if the beam intersects that polygon. If it does, the range is calculated and 
registered for that sonar beam heading. The sonar is assumed to be attached to the vehicle 
with axis of rotation parallel to the body-fixed z-axis which will undergo the same motions 
the vehicle experiences. Therefore, the unit vector, e, describing the beam direction in 
body-fixed coordinates is 



e 



cos{\f/S 
< sin(y/^) > 
0 



(6.36) 



where is the heading angle of the sonar head which can be stepped in increments of 

0.9, 1.8, or 3.6 degrees. Figure 6.8 shows the vector relationships between the vehicle and 
polygonal environment. The point of intersection, P^, in global coordinates is given by 

P, = Ro + h(r^ + r) (6.37) 

where h is the local to global transformation matrix. The vector r, is the position of the 
sonar head, and r is the sonar range vector both expressed in body-fixed coordinates The 
vector r may also be expressed as er where r is the scalar magnitude of the range which 
can be calculated from 




(6.38) 
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The range along with the sonar heading angle y/^., will be the sonar information available to 
the vehicle controller. 

Since the controller relies on an uncertain dynamic model using the best estimates of 
the vehicle parameters, some mismatch will be present with the actual system. Therefore, 
the simulation developed two models, one using what is considered the "true" parameter 
values, and the other, estimates of these values, employed by the vehicle controller between 
sonar updates. The simulator more closely represents the situation for in-water tests since 
only estimates of the acUial vehicle parameters are available and robustness issues are then 
elucidated. The sonar head position, and consequently the range values returned, are based 

on movements from the "true" model. The parameters which were allowed to differ are the 
masses, M^, M^., /^ , the damping, , b^., and the thruster gains, a^, or,., a^. 

A Silicon Graphics workstation was used for the simulation. During execution, the 
polygonal environment and vehicle movements were animated and displayed to provide a 
visual verification of the control technique. The simulation control modules were designed 
to have exactly the same structure and functionality as would execute in the vehicle 
Execution Level. This enabled no code changes to be required between the simulator and 
the vehicle, thus significantly reducing the software debugging process for the in-water 
tests to follow. 

F. SIMULATION RESULTS 

The simulation comprised of five commanded poses with respect to the target as 
listed in Table 6.1 and shown graphically in Figure 6.9. 

Table 6.1 Commanded Mission Poses 



Pose 


(ft) 


(ft) 


¥com (rad) 


1 


-7.0 


-3.0 


0.0 


2 


-7.0 


0.0 


0.0 


3 


-7.0 


3.0 


0.0 


4 


-7.0 


0.0 


0.5236 


5 


-9.0 


-3.0 


0.0 
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The following tables give the parameter values used in the vehicle model and the 
sliding mode controller gains. 

Table 6.2 Parameters for Vehicle Model Table 6.3 Sliding Mode Controller Gains 



Parameter 


Value 


Unit 


m 


435.0 


lb 


^ax 


43.5 


lb 


m^, 


348.0 


lb 


L, 


53.60 


Ib-ft-sec^ 


^zza 


53.60 


.u f 2 

Ib-ft-sec 


K 


1.33 


Ib-sec2/ft2 


by 


17.0 


Ib-sec2/ft2 


b^ 


55.87 


.u f 2 

Ib-ft-sec 


a. 


0.025 


lbA^2 


dy 


0.004 


lbA/2 


OCy 


0.006 


lb-ftA^2 



Parameter 


Value 


Unit 




0.20 


rad/sec 




0.20 


rad/sec 




0.20 


rad/sec 




0.5 


m/sec2 


Tlv 


0.3 


m/sec2 


Tly 


0.20 


rad/sec2 


<l>. 


0.2 


ft/sec 


<l>v 


0.3 


ft/sec 




0.20 


rad/sec 



Note: a^i, = - ocj,,, where /,, is the distance from the mass center of the vehicle to 

the center of the lateral thruster axes which is the same for both thrusters. 



The controller used step inputs in position while the commanded rates were set to 
zero. The control phase transitions were based on the position and rate errors in the X , Y, 
and y/ directions. Using these three directions, the error surface was formulated as 



0(T) = 






wJeCDl 



(6.39) 



where 



e{T) = 



y^coJT) 



X, {T) 

Y, .(T) > 

¥(T) 



(6.40) 
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This formulation differs from the definition given in Chapter IH for signal transition since it 
does not include the velocity error. This was done because the transition was based on 
position errors from the sonar ranges, not from the model estimates since the model will 
generally predict a very smooth trajectory to the set point prior to sonar update, regardless 
of the actual vehicle position. This led to the monitoring of the transition only at time of 

update, T, and the velocities near the terminal phase of the maneuver were assumed to be 
small. The parameters for the error equation used were = Ggy = 0.5 feet, = 0.1 

radians, and = 1.0. 

The simulation results assume no parameter mismatch between the model for 
control and the true system and uses a control loop step time of 0.2 seconds (5 Hz). This 
value, instead of the usual 10 Hz rate was used based on preliminary timing tests using the 
vehicle CPU. Addition of the dynamic model and sonar control overheads required the 
control step size to be doubled in order to complete all the calculations. The X and Y 
position response with respect to the target is shown in Figure 6.10. The set points 
commanded in Table 6.1 are achieved quite easily and the control model is predicting very 
closely the actual position of the vehicle given by the sonar (asterisks). It should be noted 
that the slight discrepancy of position between the model and sonar is due to a time lag 
between target identification and model update. Recalling the identification methods of 
section 6.4, a new segment must be declared before the prior one is analyzed. This causes a 
delay of approximately 2.0 seconds before the coordinates of the cylinder are calculated and 
passed to the model for update, at which time the state has evolved 2.0 seconds beyond the 
time of observation. Fortunately, the delay is too small to cause instability for such a slow 
moving vehicle. The range and bearing angle of the sonar is shown in Figure 6.1 1. It 
clearly shows the dynamic tracking performance of the head throughout the maneuver. 

G. EXPERIMENTAL RESULTS 

The in-water experiment used the same vehicle and controller parameters used in the 
simulation along with the same transition procedure. The vehicle was commanded to 
submerge to a depth of 1.2 feet using vertical thrusters as detailed in Chapter V. Once this 
depth was reached, the ST 1000 sonar was activated and scanned clockwise until the target 
(cylinder) was identified. At this time, the first pose (1) was issued and the vehicle started 
the controlled maneuver. 
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Figure 6.12 shows the position response results where the upper trace is Y(t) and 
the lower X(t). The position calculated from sonar at update, X^{T) and Y^{T) are shown 
with circles and asterisks respectively. Examining the response for X{t) it is evident that 
the model for the longitudinal direction is in error since the predicted position at the time of 
correction is about double that calculated with the sonar. This mismatch has been attributed 
to errors in the development of forward thrust on the vehicle in transient conditions 
(Healey, et. al., 1995). The absence of shrouds around the stem screws appears to lead to 
an unmodeled transient force lag is present that is common with open propellers. Since this 
lag was uncompensated, and the control was dictated by the model predictions between 
position updates, large voltage commands to the screws were of too short a duration to 
build up sufficient force on the vehicle as shown in Figure 6.14. The performance was 
further degraded by the estimated position and rate feedback from the model. As these 
values were assumed to be nearing the set point pose, the controller actually reversed the 
propellers (negative voltage command) in an attempt to slow the vehicle. This effect can 
also be clearly seen in Figure 6.14 between the time 44.6 seconds and 55.0 seconds, the 
time of the position update from the sonar. The prediction of the lateral movement, Y(t), is 
much more precise since the cross-body thrusters are shrouded due to their tunnel design, 
and the model parameters are well established. 

1 . Thruster Lag Analysis 

In an effort to confirm the effect and quantify the inherent propeller force lag, 
attention was returned to the simulator used before. The equation of motion for the 
longitudinal direction was modified to include a first order force lag, and in discrete form is 

( 6 . 41 ) 

where T is the control loop time step, T is the time constant, is the longitudinal force 
commanded by the controller, and F^^ is the resulting lagged force applied to the "true" 

model. The model used by the controller did not have this lagging effect included in it's 
formulation since this was not the case during the in-water experiment. Various values of 
the time constant, t, were used in the simulation to match the results obtained from the 
actual experiment. The value which provided the best match was 14.0 seconds. The 
comparison between the actual and simulated responses for the first pose maneuver is 
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shown in Figure 6.15. Very close matching is evident for this choice of time constant. As 
time proceeds, the match begins to diverge which is caused by two reasons. The first is due 
to the fact that the cylinder is only approximated with flat polygons in the simulation which 
causes the identification algorithm to behave differently for a given number of scans. The 
second and most significant is the lack of noise and outliers seen in the actual test and not 
modeled in the simulation. This causes the simulation to reaquire the target very rapidly 
which causes the update time for the simulation to slowly but consistently out pace the 
results from the in-water experiment. With this better understanding of the vehicle model, 
the lag can be compensated to improve the performance and will be the subject of future 
work. 

H. HIGH SPEED TRACKING 

Although the results from the previous section are acceptable, they were still in need 
of improvement, especially in the area of position update rate. The target was found on 
average every 10 seconds and caused a rather long time to complete the 5 pose maneuver. 

A new high speed algorithm was therefore studied using the standard target locating 
technique to initially acquire the cylinder but instead of activating a wide sweep of the 
sonar, the sweep direction was directed to the calculated center of the cylinder. To correct 
for the relative motion, an algorithm to maintain lock was devised and is shown pictorially 
in Figure 6.16. The use of a sweep width of 3.6 and step size of 1.8, consists of only a 
left, center, and right heading which enables the direction the target has apparently moved 
relative to the sonar to be determined. As seen in the figure, the scan direction y/^j is 

changed depending on which beam loses lock, either the left or the right. If lock is lost 
from the left beam, y/^j is incremented by 3.6 degrees and decremented by the same 

amount, if the right beam fails to hit the target. This method assumes relatively slow vehicle 
speed to prevent exceeding the bandwidth of the sonar. This must be seriously considered 
if the motion of the vehicle becomes too fast, the sonar will not be able to slew the head fast 
enough to maintain lock, especially near the target due to the high angular rate of change 
between the target and vehicle. 

Since the control will rely on the sonar range signal, the velocity of the vehicle must 
be determined by using the standard form of the kinematic Kalman filter with thresholding 
used throughout this work. The formulation will also be useful for rejecting the large range 
values sensed when the sonar beam drops off the target and for any outliers caused by 
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multi-path or noise. The position of the vehicle is determined by using the transformation 

Eqn. (6.27), but in this case, it is used each time step for update, not at the asynchronous 
time T . Another modification is the deletion of the cylinder radius, r^,, in the calculation 

of the range to the cylinder, R^.^, (Eqn. (6.27)). The new form of Eqns. (6.25) and (6.27) 
become 



where the sonar range to the target is 



Kylit) = 



fi?„,(/)cos(v/(t) + W.M))' 

|^n,(Osin(i/(0 + V^,(/)) 



(6.42) 



(6.43) 



With these modifications and the fact that the update is now assumed to be 
synchronous, the block diagram for the control changes from the representation in Figure 
6.6 describing asynchronous updates, to appear as in Figure 6.17. The values of XJt) and 
YJt) are individually filtered and if either residual exceeds a value of 1.0 feet, the 
corresponding range is assumed to be an outlier or caused by loss of lock. In this case the 
residual is zeroed and the estimates for position and velocity are obtained from the 
kinematic model alone, as was done for wall servoing described earlier. The filtering 
generates estimates for the position and rate vectors given by 



and 



RJt) = 



X„(r)| 

t(t)\ 



RJt) = 




(6.44) 



(6.45) 



respectively. The range was transformed to X,.(r), YJt) before filtering instead of filtering 
the range then transforming, since previous work indicated this ordering provides a much 
smoother result, (Zinni, 1995, Scrivener, 1996). 
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Previously it was assumed that the range information that arrived asynchronously 
from the cylinder identification algorithm was extremely accurate. This is not a bad 
assumption since the algorithm is quite robust and a single value of range and sonar 
heading is obtained per update. However, attempting to use the ranges obtained 
synchronously from a beam that strikes a cylindrical object at discrete angles can cause 
relatively large range changes from heading to heading. This is especially acute near the 
edges, and the filter predicts very large range rates from these. Reduction of the Kalman 
gain to remove this undesirable effect can introduce lags which can lead to instability of the 
entire control system. A more appropriate method is to continuously fuse the position and 
velocity values from the kinematic filter with the estimates obtained from the model. 
Therefore, the variances from the model and sonar should be non-zero to provide additional 
conditioning of the position and velocity. These assumptions form the basis for a new set 
of Kalman filter equations supplanting Eqn. (6.30) given by 



X^iO = (7 - K)X(t) + KX,.(t) 

Y^(t) = (7 - K)ht) + KY^Xt) 
where the velocity is also calculated using 

= (7 - K)X(t) + KX^it) 
Yf{t) = (7 - K)ht) + xiio 



(6.46) 



(6.47) 



where the subscript "/" indicates a fused value and the gain K is obtained from Eqn. 
(6.31). 

If target lock is lost for more than three consecutive pings, the tracking algorithm 
has lost lock for a longer period than can be remedied using a sweep width of 3.6 degrees 
and reaquisition of the target must be undertaken. During this time, the variances of the 
sonar range is set to 1.0 and 0.0 for the model respectively to force reliance solely on the 
model estimates for control. At this time the kinematic Kalman filter for the sonar range is 
deactivated to prevent divergence. Currently, the action taken to reaquire the target involves 
expanding the sweep width to ever increasing multiples of 3.6 degrees. During this time, 
the model is used to predict the location of the target using Eqn. (6.28) from the estimates 
of position. If lock is not obtained within a specified time, the mission is terminated. If lock 
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is re-established, the kinematic Kalman filter is reactivated and initialized to the new values 
of position. 

Figure 6.18 shows the simulated position response in the Y direction using the new 
method with the commanded poses and vehicle/controller parameters of section 6.9. Since 
it is extremely important to regulate the vehicle speed to keep the motions within the 
bandwidth of the sonar movement, command generators were used for all three control 
directions, X, Y, and y/. Comparing with Figure 6.10, the mission time is not 
significantly reduced since the maneuvers were dictated by command generators which had 
a 20 second elapsed time specification for each segment. This had to be done otherwise 
lock on the target would have been lost if a faster response was chosen. Analysis of Figure 
6.19 shows that the sonar head responds to the loss of track by correcting the scan 
direction, in accordance to the rules defined above. 

The method described above has been used to obtain some preliminary data in the 
test tank. The results were very promising as long as lock was maintained on the cylinder, 
but once lock was lost, the algorithm showed poor performance in re-acquiring lock. This 
was mostly due to shortcomings in the tracking software and the performance of the 
ST 1000 sonar was degraded due to a suspected electronic temperature fault in the head. 
Further software enhancements and hardware repairs will be needed to overcome these 
problems. 

I. CONCLUSIONS 

The results of these experiments have shown that it is possible to navigate an 
underwater vehicle in a local area using an acoustic sensor for position information. The 
accuracy of the model used between updates is moderately satisfactory and can allow for 
time varying currents. However, some additional model adjustments could be made to 
compensate for the force lag in the longitudinal direction during transient thrust conditions. 
This undesirable effect could also be alleviated physically by the addition of shrouds 
around the stern screws which should bring the performance up to that of the lateral 
thrusters. While these results were taken in a tank environment, another improvement 
would be to fuse the model with an INS system in between updates from the sonar and 
then fuse that estimate with the sonar data to obtain a smoother averaging at update time. 
This would allow for compensation of wave induced disturbances while retaining the 
positioning precision found. Since the sonars are mechanically scanned, and a delay of up 
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to 10 seconds between position update is common, use of an electronically scanned or 
multi-beam sonars may be preferable although our experience to date has been that cross- 
talk between beams can be a serious problem. 

To increase the response time, a continuous target tracking algorithm was 
simulated. This provided extremely favorable results but will need further modifications to 
successfully operate in the actual vehicle. 
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CHAPTER VI FIGURES 
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Figure 6.1 ST 1000 Sonar Image of a Cylinder and the NFS Test Tank. 
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Figure 6.2 Object Segmenting Algorithm Flow Diagram. 
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Figure 6.3 Object Segment Shape Algorithm Flow Diagram. 



Sonar 

Head 




Vehicle 



ure 6.4 Position Vector Definitions for Local Area Navigation. 
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Figure 6.5 Sonar Scan Patterns for Maneuvers with Respect to a Cylinder. 
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Figure 6.6 Sonar with Model Control Block Diagram. 
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Figure 6.7 NFS Test Tank with Cylindrical Target Polygonal Representation. 
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Figure 6.8 Position Vector Definitions for Simulation Environment. 




Figure 6.9 The Five Commanded Poses. 




Figure 6.10 Position Response vs. Time (Simulation). 
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Figure 6.1 1 Sonar Range Returns and Heading Angle vs. Time (Simulation). 
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Figure 6.12 Position Response vs. Time (Experimental). 
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Figure 6.13 Sonar Range Returns and Heading Angle vs. Time (Experimental). 
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Figure 6. 14 Stern Screw Control Voltage and Position Response vs. Time. 
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Figure 6. 15 Comparison of Force Lag Between Simulation and Experimental vs. Time. 
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Figure 6.16 Sonar Target Tracking Method. 
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Figure 6.17 Sonar with Model Control Block Diagram. 
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Figure 6.18 Position Response vs. Time (Simulation). 
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Figure 6.19 Sonar Range Returns and Heading Angle vs. Time (Simulation). 
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VII. CONCLUSIONS AND RECOMMENDATIONS 



The main puq)ose of this work has been to develop, demonstrate, and validate an 
open architecture, three level control system for the real time control of an autonomous 
underwater vehicle, (AUV). As applied to underwater vehicles, the three levels have been 
named the Strategic, Tactical, and Execution levels. At the top is the Strategic Level It is an 
asynchronous, discrete event system managing the progress of the mission phases, and 
implemented in the rule based language, Prolog. Below this is the Tactical Level, written in 
the language C, which computes data necessary to coordinating the control modes required 
for each mission phase and is also asynchronous. Finally, the C language Execution Level 
performs the synchronous motion control of the vehicle. Since sequencing mission phases 
is inherently an asynchronous process, and operates with longer time scales than the 
synchronous servo controllers, there is a convenient separation to run them using different 
processors. The natural division of the synchronous and asynchronous tasks maps well 
into the separation of symbolic and numeric operations and is well suited to multiple 
computers. With this flexibility, each system may be chosen based on their particular 
attributes with regard to computer languages, operating systems, timing constraints, and 
processor speeds. 

Combining the theories of discrete and continuous time control techniques, the 
Hybrid control system allows multiple task robot behaviors to be accomplished. Particular 
behaviors demonstrated included submerging, heading, and longitudinal control, along 
with local area positioning of the vehicle using acoustic sensors. Each control scenario 
described above was subject to simulation studies followed by experimental verification 
using the NPS Phoenix vehicle. 

A. SUMMARY OF CONCLUSIONS 

1 . Chapter II Conclusions 

Results from Chapter II have shown that the formulation for a MIMO Sliding Mode 
controller performs very well in the slow speed maneuvers supporting robotic behaviors. 
The control method includes the use of a weighted minimum norm for the solution of the 
inverse dynamics. Flexibility in adjustment of control effort between multiple redundant 
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actuators is thereby available and supports automated control reconfiguration for enhanced 
reliability. The simulations have also shown that adequate control authority is needed when 
performing path tracking maneuvers. So far the weights for the inverse solution of the 
input gain matrix have been either all unity or some unity and some zero. Further work is 
needed to formulate the weights for the control surfaces and thrusters to be a function of 
vehicle forward speed. Using this, it should be possible to avoid control saturation during 
any given maneuver. 

A robustness analysis was also presented which included modeling inaccuracies in 
the mass, dynamics, and input gain parameters. The analysis provided a design procedure 
to ensure stability despite the uncertainties. The results of this work have emphasized the 
need for control designers to clearly understand the performance limits of the vehicle to be 
controlled. 

Although, the control design methodology presented appears robust and well 
behaved in simulation, other real factors not included are lags in thruster response. If 
relatively long lags are present, these effects should be included in the general analysis. 

2 . Chapter III Conclusions 

The conclusion of the work in Chapter III has indicated that complex behavior can 
be readily coordinated through Strategic Level rules, that are easily modified. Two kinds of 
predicates have been introduced. The first provides commands for particular vehicle action, 
while the second requests data for the evaluation of mission state transitions. 
Communication through Tactical Level software to the Execution Level controllers is a 
simple but convenient way of commanding stable responses of the vehicle. The design of 
well behaved control laws and functions at the Execution Level is essential and is 
accomplished through careful attention to the sliding mode control law specifications. 
Reactivity, failure recovery, and even human interfacing within the controller can take place 
at any level. 

3 . Chapter V Conclusions 

The results of Chapter V have shown that the Phoenix can be precisely controlled in 
the test tank environment using thrusters. Development of sliding mode controllers for 
submergence, heading, and longitudinal control have been proven to be robust and have 
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shown to provide conveniently tuned, exceptional vehicle performance. The flexibility of 
the Strategic Level rules has allowed many different control scenarios to be quickly tested 
and evaluated without major code modifications. Using command generators provided 
extremely precise time based maneuvering and proved very effective when activated 
concurrently for multiple interacting control modes. 

a. Submergence Control Studies 

The results presented have shown that the depth of the Phoenix can be 
precisely controlled using Sliding Mode control of the vertical thrusters. Although 
discretization noise from the depth cell produced more control action than desired, the 
overall positioning performance is exceptional, and this problem could be rectified using a 
finer resolution A/D converter. Applying integral control has shown to be very effective in 
compensating for any deviation of the vehicle buoyancy from neutral, especially if activated 
at the proper time. Using command generators provided extremely precise, time based 
maneuvering, even in the presence of buoyancy disturbances. 

b . Heading Control 

The results of Sliding Mode heading eontrol for the Phoenix using the 
lateral thrusters has been exceptional in both step response and command generator 
performance. The tendency of the lateral thrusters to chatter has not adversely affected the 
vehicle motions. Nevertheless, using post filtering and a dead band on the thruster input 
voltage lead to a significant reduction of this undesirable phenomena. The command 
generator performance was highly accurate, demonstrated by almost perfect tracking of the 
input profiles. 



c . Longitudinal Motion Control 

The results of sliding mode wall servoing control have shown highly 
satisfactory accuracy in longitudinal positioning of the vehicle. Using a Kalman filter 
modified to reject range outliers provided very accurate and stable signal conditioning from 
the STIOOO sonar data. A comparison between sliding mode and proportional derivative 
eontrol has been presented which demonstrates the superiority of the non-linear approach. 
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Future work in this area could include the use of two sonars to enable both longitudinal and 
lateral positioning of the vehicle for more general control scenarios. 

d . Coordinated Control 

It has been shown that is a relatively easy task to program coordinated 
vehicle maneuvers since the generality of the Strategic Level rules allow the method of 
control to be determined by the Tactical Level. Accurate, simultaneous tracking of the 
command generator trajectories proved that the servo level controllers are currently very 
well tuned for the vehicle maneuvers shown. 

4 . Chapter VI Conclusions 

The results of Chapter VI have shown that it is possible to navigate an underwater 
vehicle in a local area using an acoustic sensor for position information. The accuracy of 
the model used between updates is moderately satisfactory and can allow for time varying 
currents. However, some additional model adjustments could be made to compensate for 
the force lag in the longitudinal direction during transient thrust conditions. This 
undesirable effect could also be alleviated physically by the addition of shrouds around the 
stem screws which should bring the performance up to that of the lateral thrusters. While 
these results were taken in a tank environment, another improvement would be to fuse the 
model with an INS system in between sonar updates and then, at update time, to fuse that 
estimate with the sonar data. This would allow for better compensation of wave induced 
disturbances while retaining the positioning precision found. Since the sonars are 
mechanically scanned, and a delay of up to 10 seconds between position update is 
common, use of an electronically scanned or multi-beam sonars may be preferable although 
our experience to date has been that cross-talk between beams can be a serious problem. 

To increase the response time, a continuous target tracking algorithm was 
simulated. This provided extremely favorable results in simulation but will need further 
modifications to successfully operate in the actual vehicle. 
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B. SUGGESTIONS FOR FUTURE WORK 



Additional work is needed in several areas, and falls into three main categories: 
software improvements, upgrades of the vehicle hardware, and more sophisticated mission 
capabilities. One pressing need is to develop a graphical user interface (GUI) to provide an 
easier means of programming mission control. Such a system should include features to 
prevent attempts to design nonsensical missions or activation of inappropriate behaviors. 
The system should auto-generate executable code. 

An expanded set of vehicle primitives needs to be developed to enhance vehicle 
capabilities, especially if new sensors or actuators are installed such as side-scan and 
Doppler sonar. Improved fault detection software should also be included as part of a 
Tactical Level engineering module for on-line diagnostic state of health monitoring. 
Replacement of the Tactical Level error filters with softer techniques for decision making 
should also be explored. Using elastic rather than crisp constraints allows decisions to be 
made much faster with varying levels of confidence as opposed to the more constrained and 
invariant filtering approach. Applying the above measures, will also enable reconfigurable 
servo level control schemes through automatic redistribution of control effort if certain 
actuators fail. 

In the area of hardware improvements, implementation of radio Ethernet into the 
vehicle will allow tetherless remote communications while surfaced. To communicate with 
the vehicle while submerged, recent advances in underwater acoustic data transmission 
holds great promise and preliminary results using this technology have been positive. 
Using distributed processing in the Execution Level can allow a much better balance of 
computational requirements. For example, relegating the signal processing tasks to a 
dedicated processor associated with each sensor will significantly reduce the computational 
load on the controlling computer. For operations in the ocean environment, increased 
vehicle speed and endurance will be required, and can be achieved by using more powerful 
propulsion systems along with readily available, higher energy density batteries. 

One of several missions that need to be investigated is the automatic garaging and 
re-powering underwater. While current studies funded by ONR in the AOSN program are 
aimed at addressing this problem, the design of capture mechanisms, precision homing, 
and devices for power and data transfer are in need of further research. 
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It is important to evaluate the capabilities of the control architecture for performing 
simulated mine countermeasures missions while operating in the shallow water ocean 
environment. In particular, it will be important to evaluate the influence of waves on sonar 
image distortion and on the ability of the control to stabilize the induced vehicle motions. 
An additional area for future research in shallow water environments will be to establish 
within the Tactical Level a machine leaning capability for on-line sea state estimation. 
Learning from the wave induced motions, control techniques for anticipatory compensation 
may assist in improving the precision of positioning. 
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APPENDIX A. EQUATIONS OF MOTION FOR THE NPS 

PHOENIX 



The equations of motion and parameter values used to simulate the dynamic 
behavior of the NPS Phoenix is given in this appendix. 

A. PHYSICAL PARAMETERS 



W, Vehicle Weight = 435.0 lbs 
B, Vehicle Buoyancy = 435.0 /^5 
/, Characteristic length = 730 ft 



= 2.7 lb -ft -sec- /„ = 42.0 Ib-ft-sec^ = 45.0 lb -ft -sec' 
= 0.0 Ib-ft-sec^ f ^ = 0.00 Ib-ft-sec^ f. = 0.00 Ib-ft-sec^ 



x^.,. Bow Vertical Thruster Offset from C. G. = 1.420 ft 

x„.,, Stem Vertical Thruster Offset from C. G. = -1.420 ft 
x^i , , Bow Vertical Thruster Offset from C. G. = 1 .920 ft 

x^i,. Stem Vertical Thmster Offset from C. G. = -1 .920 ft 
y,^, Left Screw Offset from C. G. = -0.330 ft 

y„, Right Screw Offset from C. G. = 0.330 ft 



Xq, X Coordinate of C. G. From Body-Fixed Origin 
, y Coordinate of C. G. From Body-Fixed Origin 
Zq, z Coordinate of C. G. From Body-Fixed Origin 
Xg X Coordinate of C. B. From Body-Fixed Origin 
yg y Coordinate of C. B. From Body-Fixed Origin 
Zp z Coordinate of C. B. From Body-Fixed Origin 



0.010 ft 
0.000 ft 
0.042 ft 
0.010 ft 
0.000 ft 
0.000 ft 
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B 



CONTROL INPUTS 



Fu 


Left Screw Force 


{lbs) 


Frs 


Right Screw Force 


{lbs) 


Fm, 


Bow Lateral Thruster Force 


{lbs) 


F.U 


Stem Lateral Thruster Force 


{lbs) 




Bow Vertical Thmster Force 


{lbs) 




Stem Vertical Thmster Force 


{lbs) 




Bow Rudder Deflection 


{rad) 




Stem Rudder Deflection 


{rad) 




Bow Plane Deflection 


{rad) 




Stem Plane Deflection 


{rad) 



C. NON-DIMENSIONALIZED HYDRODYNAMIC COEFFICIENTS 



Surge Hydrodynamic Coefficients : 



II 

o 

b 


x; = 0.0 


X 

II 

o 

b 




= 0.0 




= -0.01743 


x; = 0.0 


x; =-0.00282 


x; = 0.0 




= 0.0 




= 0.0 


x; = -0.00753 


II 

o 

b 


II 

o 

b 




= 0.0 




= -0.01018 


X' =-0.01018 


X' =-0.01018 

^hp^hp 


X,„ = -0.4024 









Sway Hydrodynamic Coefficients: 





= 0.0 


n 


= -0.03430 


II 

O 

b 


Y' 

^ 6sr 


= 0.01241 


y'r 


= -0.00178 




= 0.0 


C =0.0 


Y' 

^ 5hr 


= 0.01241 


K. 

K 


= 0.0 
= 0.0 




= 0.01187 
= 0.0 


r; =-0.10700 

r = 0.0 
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Heave Hydrodynamic Coefficients: 



2' 


= -0.00253 




= -0.09340 


z: 


= -0.78440 




= 0.0 




= -0.07013 


z; 


= 0.0 




= 0.0 




= 0.0 


Z' 

^5sp 


= -0.02110 




= 0.0 




= 0.0 


z;. 


= -0.02110 


Roll Hydrodynamic Coefficients: 








= -0.00024 


K 


= 0.0 


K 


= 0.0 




= 0.0 


K 


= -0.00540 


Kr 


= 0.0 




= 0.0 


K 


= 0.0 


K 


= 0.0 




= 0.0 




= 0.0 


c. 


= 0.0 


Pitch Hydrodynamic Coefficients: 






M' 


= -0.00625 




= -0.00253 


Ml 


= 0.05122 


M' 

PP 


= 0.0 


K 


= -0.01530 


Mi 


= 0.0 


Kr 


= 0.0 


K 


= 0.0 


Ml 


= -1.7664 


M'rr 


= 0.0 


Mi 


= 0.0 


Ml 


= 1.3260 



Yaw Hydrodynamic Coefficients: 





= 0.0 


K 


= - 0.00047 


K. 


= 0.0 


P<i 


N' 


= 0.0 



K 


= - 0.00178 


K 


= 0.0 


K 


= - 0.00390 


K 


= 0.0 



K. 


= 0.0 


wp 


Kr 


= 0.0 


K 


= - 0.00769 


Ni . 


= 0.0 



Kr = - 1-7663 
Kr = 1-3259 
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Mass Matrix: 





m 


- 


0 


0 


0 


mZc 




-myc 






0 


m-Y, 


0 -1 


[mZc + I'J 


0 


mx^ - K, 






0 


0 


m - Z„ 


myc 


-(mx^ + Zj 




0 


M = 




0 


■{mZc + X,.) 




L-x, 


-u 


-{1 






mzc 


0 


+ 

1 


-u 


A. - 




-A. 






■myc 


mXfj - N^, 


0 


{L + A'.) 


-A. 


I, 


. - A'. , 




~m - 


0 


0 


mZc 


-myc 




0 






0 


m - n 


0 


0 


mxc - Y^ 


-('nzc + Y^ 






0 


0 


m - Z„ 


-(mxc + Z.) 


0 




>nyo 


M' = 




mZc 


0 


+ 

1 


A v - 


-A.: 




-K 






-myc 


mxc - N^. 


0 


-A: 




-(' 


« + A^,.) 




- 


0 


-{mZc + K,) 


my’c 


-L 


-(A. + X,) 


L-x, 


Input Gain Matrix: 
















■ 0 


0 


0 0 


1 1 


0 0 


0 


0 ■ 






4*\Yshr 


0 


0 


0 0 


0 1 


0 


1 


g ■- 




0 

0 


^l^l^Shp 

0 


0 u\n\Zgsp 

0 0 


0 0 
0 0 


1 0 
0 0 


1 

0 


0 

0 






0 


^H^shp 


0 u\u\Mg^^ 


0 0 


-^hv, 0 


0 








0 


«l«l^5vr 0 


1 

p^* 

1 


^ ^hU 


0 
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0 


0 


0 


0 


1 


1 


0 


0 


0 


0 


MYshr 


0 




0 


0 


0 


0 


1 


0 


1 


0 




0 


M^Ssp 


0 


0 


1 


0 


1 


0 


0 




0 


M^Ssp 


0 


0 


-Xhv, 


0 




0 


M^Shr 


0 




0 


-Yh 


-Yn 


0 


^hU 


0 





D . EQUATIONS OF MOTION 

Surge Motion Equation: 



m[u 


- vr + wq - X(j{q 




- r) + Zcipr + q)] 


= 


-l'\X'p- + X'q^ 

2 L pp^ 


+ xy + xyr] 




+ 


+ x:,wq + x;vp + x:,vr + 




+ 


“'•(XV + XV V 


)] 




+ 


+ X’„w- 


+ «I»I(XV(5; + 


+ \s.K + 


- 


(W - B)sind + F,, 


+ F,, + X„«l“l 





Sway Motion Equation: 

m[v ^ ur - wp + Xcipq + r) - y^ip' + r^) + z^qr - p)] 

= + y'ri- + y;,pq + y’M 

+ |/^[y;v + Y'^up + Y[ur + Y[^vq + Y^^wp + F',wr] 

+ ^/^[y>v + Cvw + u\u\(Y'j,^^ + 

+ ^ f{Q,,/?(A:)(v + xr)^ + CjMx){w + xqY] 

+ (ly - fi)co50sin^ + F^,, + F,/, 
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Heave Motion Equation: 



m[w - uq + vp + Xcipr - q) + y^iqr + p) - Zgip^ + q^)] 

= |/'Jz'« + z;p' + z'^pr + zy] 

+ f'lz'vi. + + zy + zy] 

+ |;'[z:.v> + zy + »|u|(z;/„ + z;/.,)J 

- ^ [[Q.*(jr)(i’ + xrf + cyx)(if - xq)‘] 

+ (W - B)cos(pcos6 + + F,^,, 

Roll Motion Equation: 

hp + + Ix^ipr - q) - Az(?’ - r^) - hz^pq + 

= K'r + + F>] 

+ ^/^[f'v+ F> + F>r + F;vg+ F;vvp + F:,wr] 

+ //i[Vg(m' - uq + vp) - Zg(i^ + Iff - ^f’p)] 

+ - ygB)cos<l)cosd - {ZqW - ZBB)cos6sin^ 
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Pitch Motion Equation: 



A<? + (^x - h)P^ ~ + P) + fy,iP^ - r) + I^.{p^ - r^) 

- m[X(j{w - uq + vp) - Zq{u - vr + wq)] 

= f + M'„p^ + M>r + M;/] 

+ + M'^uq + M'.pVp + 

+ §''[«,>' + K“« + “HKA + 

" ? PfCjjWxXv + xr)’ + Cj.6(x)(u> - xqf\ 

- {XqW - XsB)cos<pcosd - {ZgW - ZBB)sin6 - Xb,,Fbv, ~ 



Yaw Motion Equation: 

// + (/, - I,)pq - l„ip^ - q^) - IJpr + ^) + IJqr - p) 
+ m[X(j(v + ur - wp) - y’Giu - vr + wq)] 

= ^i\n;p + N',r + N’„pq + 

+ ^/‘(W'v + N'^up + N’,ur + Nl.^vq + W',vt>p + W;,,ur] 

+ ^1 \n:uv + N'„xm + u| 4N;„5„ + JV;,5.,)] 

- ^ f[cii,Mx)(v + xrf + Cj.iCxXM' - xqf] 

- {XgW - XBB)sin(pcosd - (VcW - yBB)sind 

^hll^hli ^.sll^sll yis^lx yrs^rs 
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Euler Angle Rates and Global Positions: 



X = + ucosXj/cosO + v[cos\ifsindsin^ - siny/cos(l)] 

+ w[cosy/sin6cos(l) + siny/sin^] 

Y = + usinyfcosd + v[siny/sin6siny) + cos\j/cos(l)] 

+ w[siny/smdcos<l) - cosy/situj)] 

Z = - usind + vcos6sin(l) + wcosOcosip 

0 = p + q sin (p tan 6 + rcostptand 
6 = qcostp - rsintp 

. _ (gsintp + rcostp) 

^ cos 6 



Cross-flow Velocity: 



= ^[(v -I- xr)- -I- (w - xqf] 
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APPENDIX B. COMMAND GENERATORS 



To control the positional motions of an underwater vehicle, position, velocity, and 
acceleration command generators may be required. In the one-dimensional case the 
commands are obtained from the specification of a desired final zero velocity and 

acceleration position given an initial zero velocity and acceleration position. The elapsed 
time to complete the maneuver, 7y, is determined from the desired distance to travel which 
is the difference between the initial position, Sq, and the final, Sj, along with and 
s^, the maximum allowed vehicle velocity, and acceleration respectively. These values 
also determine the duration of T^, the time of constant velocity between acceleration and 
deceleration. To avoid any discontinuities in either of the command arrays, the acceleration 
curve must be at least third order, and gives a velocity curve of order 4 and a 5th order 
position curve as shown in Figure B.l. 

Given a third order acceleration command equation of the form 

m = At^ + (B.l) 



the coefficients A and B must be 






2s 

^ max 






(B.2) 



to force a zero slope at / = 0 and t = . The duration of is calculated using 



T = 

^ cv 



a2 ^ 






(B.3) 



^max / 



If < 0 and the value of Sj - is too small for the given specifications of and 
this requires that be reduced until the constant velocity interval is exactly 0 
given by 



^max 



hf 


- ^0 


’^rnax 


2 



T„. = 0 



(B.4) 
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The elapsed time during acceleration to is 



r. 






2s 



max 



S 



max 



(B.5) 



and the value of Tj^ is determined. 

If both the distance to travel and the total time to complete the maneuver is 
specified, the value of must be of sufficient duration such that 



Tf > 




To determine the smallest possible maximum acceleration, for a given time, it may be 
calculated using 



r /" '^0 

(B.6) 

V 

which will produce a velocity curve with no region of constant velocity (i.e. = 0). 

The position, velocity, and acceleration profiles for the five time intervals shown in 
Figure B.l are given by 

For 0 < t < : 

2 



s{t) 

s{t) 

's(t) 



a 



a 




(At^ - Bt^) 



J 




(B.l) 
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Ts 

For < t < T, : 

2 ‘'wm 





B /_ ' 


,5 5 - 1 


— (F. - 


/) - — \T. - t 


+ i t - 


[l2^ w 


1 20^ 


f ^max 

^ nuix J 



m = + f (7-,^ - >)' + (B.8) 

m = a[A{T,_ - ,)’ - - ,)') 



For < t < T, : 

s(t) 

s(t) 

s(0 




\ 

Sn,a, 

^maj( J 

(B.9) 



For T, < t < T, + 

' ' 2 



[ A f „ 


B j 


5 \ 


(/ - 3 


/) + — U - 5 


^,) + kj - 


12^ 


” 20^ 


/ / max 

^ max J 



m = a(^-|(r - T,y + |(r - T,)‘ + (B.IO) 

m = «(-/!(< - 7-,)' + B{l - T,y). 
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For T, + 



T, 

•’ffL 



< t < T, 




where 



T, = r, + and T. = 2T^ + F,, 



and a is a sign coefficient based on 




(B.ll) 
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s 




2 





B.l Command Generator Profiles for Position, Velocity, and Acceleration. 
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APPENDIX C. MINIMUM NORM SOLUTION 



Given a system of equations 



gu=f (Cl) 

where g is size fixm, u is mxl, and / is «x7, and if m>n, an infinite number of 
solutions exist. The minimum norm solution can be derived using Lagrange multipliers and 
minimization of the norm of the solution vector, |«||‘, subject to the constraint 
gu - f = 0. This can be written in terms of a performance index as 

J = + X(gu -f). (C.2) 

The minimum of J with respect to « can be found by 

^ = u + {Xgf = 0 = u + g^X^ (C.3) 



which occurs at u = -g^X^. 

The minimum of J with respect to X is simply 

^ = 0. (C.4) 

oX 



Substituting (C.3) into (C. 1) yields 



A" = -igg^r'f. (C.5) 

And substituting (C.5) into (C.3) provides the minimum norm solution 

u = g'^igg^T'f- (C.6) 
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The above treatment gives equal weighting to each m, of the solution vector. This is 
not always desirable and a modified form of (C.6) can be derived which weights each 
Define a weighted solution vector 



u = Wu 



where VT is a diagonal weighting matrix of the form 



W:: = 



(C.7) 



(C.8) 



This implies u] is a fraction of the maximum value of «,.The weighted performance index 
becomes 



J = u^u + X{gu - f) = u^W^Wu + X(gWu — f) (C.9) 



and 



^ = 0 = Wu+ 
ou 



(CIO) 



The minimum of J occurs at m = -W 'g^X^ 



^ = 0 
dX 



(C.ll) 



Substituting (C.IO) into (Cl) yields 



A' = -{gyv-'g'T'f 



(Cl 2) 



And substituting (C.12) into (C.IO) provides the weighted minimum norm solution 



u = \r'g‘(gw-'g‘ )-f 



(Cl 3) 
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For diagonal W, 



= 



= M. 



(C.14) 



Therefore, to remove the contribution of any m, from the solution vector, set it's to 
zero in W~‘ . To avoid division by zero in W, rewrite (C.13) as 

u = Wg^(gWg^r'f (C.15) 



where W = W '. 
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APPENDIX D. NPS PHOENIX HARDWARE COMPONENTS 



The following is a listing and description of the major hardware components 
currently onboard the NPS Phoenix vehicle. 

A. SENSORS 

Gyroscopes 

To sense the vehicle roll, pitch and heading angular positions along with the three 
axis rates, three types of gyroscopes are used. All are manufactured Humphrey Inc. and are 
usually used for small aircraft/missile applications, and use a mechanical motor whose 
inertial angular momentum vector is fixed in a spatial direction unless acted upon by a 
torque. 

Vertical Gyro; 

This unit is used to measure the roll and pitch angle of the vehicle. The motor is 
gimbaled where the mechanical limits are 360° freedom of rotation about the roll axis and 
±80° minimum freedom of rotation about the pitch axis. The output is ±10 Vdc and is 
read by an A/D converter on the controlling computer. Model VG34-0301-2. 

Rate Gyro; 

This is a combined package of individual gimballess rate gyroscopes for measuring 
roll, pitch, and yaw rates. The range for roll rate is 360°/sec and pitch and yaw rate 
maximums are ±90°/sec. All outputs are ±10 Vdc and read by a A/D converter. Model 
RG02-2324-1. 

Free Gyro 

This unit is used to measure the heading angle of the vehicle. The angular range is 
360° continuous and has a syncro output which is converted to 14 bit digital through the 
use of a converter chip from Analog Devices Inc. The digital data is read through two 
parallel ports on the controlling computer, 8 bits from one port and 6 from the other. The 
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gyroscope is also configured for remote cage/uncage of the gimbals from the computer. 
Model FG23-7 102-1. 

Depth Cell 

The vehicle depth is measured using a differential pressure transducer with an 
operating range of 0 to 34 feet which translates to an analog signal output of 0 to 10 Vdc 
which is read by an A/D converter on the computer. The cell is located in the center of the 
forward bulkhead inside the flooded nose. The nose shields the probe from and undesirable 
flow effects from forward motion of the vehicle. The manufacturer is Psi-Tronix Inc., 
model Sll-131. 

Forward Speed Sensor (Turbo Probe) 

The vehicle speed through the water is measured by a Turbo-Probe turbine flow 
meter, manufactured by Flow Technology, Inc. The transducer is an axial rotor mounted at 
the end of a strut which protrudes through the bottom of the nose. A cowling around the 
rotor houses a magnetic pick off which generates square wave electrical pulses at a 
frequency proportional to the rotor speed. The pulses are read by a timer card in the 
computer and converted to speed using the manufacturers supplied calibration. 

Sonars 



A single Datasonics PSA-900 Sonar Altimeter is mounted facing downward in the 
nose of the vehicle. This unit is used for depth above bottom measurements and emits a ten 
degree conical beam at a frequency of 210 kHz. The signal output is 0 - 10 Vdc 
proportional to the ranges detected up to approximately 90 feet. 

DiveTracker 



A short baseline acoustic positioning system called DiveTracker by Desert Star 
Systems is installed. The system consists of two surface transducers and one mounted to 
the vehicle. The unique capability of this system is that the vehicle location can be read by 
the onboard computer in addition to being tracked on the surface. The output from this unit 
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is read by the controlling computer through a serial link and processed for navigation. 
(Reimers, 1995, Scrivener, 1996, Zinni, 1995) 

Global Positioning System 

This unit is used for obtaining the location of the vehicle in global coordinates and 
is capable of standard and differential modes. Antennas for both are mounted on the top of 
the hull as shown in Figure 3.3 and are only usable while surfaced. The unit is connected 
to the Sun Voyager computer through a serial link for processing. 



B . GESPAC COMPUTER SYSTEM 

The Gespac boards are referred to as "Euroboards" and each have dimensions of 
approximately 4.0 X 6.0 inches and 0.75 inches thick. The 12 boards are set into a 12 slot 
G-96 bus backplane and is powered with +5, +12, and -12 Vdc and shown in the installed 
positions in Figure D.l. 

GESMPU-30H: Microprocessor Board 

Motorola 32 bit 25 MHz 68030 microprocessor with 2.0 MBytes onboard CMOS 
dynamic Ram. Four EPROM’s (Erasable Programmable Read Only Memory) chips are 
located on this board. Each have been programmed to hold essential modules for operation 
of the OS/9 operating system and configured to activate the TCP/IP network upon boot 
which enables ethernet connections from other computer systems to be made without 
relying on a console (serial) connection to the Gespac system. Once the system is up, any 
operating system modules or programs not in EPROM may be transferred to the system 
using ftp (File Transfer Program). 

GESRAM 14B: RAM/EPROM/EEPROM Memory Module 2.0 MBytes. 

2.0 MByte RAM card used for RAM disk mission data storage. 
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GESSIO-IB: Double Serial Interface Module. 



Two port RS-232-C board. Used for serial communications with the ST725 and 
ST 1000 sonars. 



GESPIA-3A: Parallel Interface Board 



Twin parallel input/output ports using TI L (0 to 5 Vdc) logic for enabling/disabling 
servo amplifier power relays for: 



Port 0: 




Bit PAO 


- Left screw 


Bit PA 1 


Right screw 


Bit PA2 


Bow vertical thruster 


Bit PA3 


Bow lateral thruster 


Bit PA4 


Stem vertical thruster 


Bit PAS 


Stem lateral thmster 



It is also used for sending signals to cage or uncage the free gyroscope and is also 
connected to the cage/uncage status lines. 



Port 0: 




Bit PBO 


Cage free gyro 


Bit PB3 


Uncage free gyro 


Port 1: 




Bit PAO 


Cage indicator 


Bit PBO 


Uncaged indicator 
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GES ADA- 1 : Analog/Digital & Digital/Analog Subsystem (10 Bits) 



A 16 channel, 10 bit analog/digital converter for; 



Channel X 
Channel Y 
Channel 5 
Channel 6 



Computer battery voltage level indicator 
Motor battery voltage level indicator 
Fore leak detector 
Aft leak detector 



Circuitry for the fore/aft leak detectors are also connected to external LEDs for visual 
reference while the program is not running. 



GESADC-2C: 16 Channel, 12 Bit Data Acquisition Module 

A 16 channel 12 bit analog/digital converter for measurements of: 



Channel 1 2 


Roll angle gyroscope 


ilOVdc 


Channel 1 1 


Pitch angle gyroscope 


± 10 Vdc 


Channel 9 


Roll rate gyroscope 


llOVdc 


Channel 8 


Pitch rate gyroscope 


±10 Vdc 


Channel 10 


Yaw rate gyroscope 


±10 Vdc 


Channel 7 


Depth cell. 


Oto 10 Vdc 


1 A: Multiple Timer 


Module (3 Modules) 





Each module contains five 16 bit programmable timers (AM9513) for both event 
counting and frequency output. Three modules are used for: 



Module 1; 



Servo motor position control for: 



Counter 1 (Output) 
Counter 2 (Output) 
Counter 3 (Output) 
Counter 4 (Output) 



Top front rudder and bottom rear rudder 
Bottom front rudder and top rear rudder 
Left bow plane and right stem plane 
Right bow plane and left stem plane 
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The outputs are pulse width modulated signals which are proportional to the desired 
angular deflection of each servo motor. 



Module 2: 



Motor rotation rate measurement of: 



Counter 1 (Input) - 
Counter 2 (Input) - 
Counter 3 (Input) - 
Counter 4 (Input) - 



Bow vertical thruster 
Bow lateral thruster 
Stem vertical thruster 
Stem lateral thruster 



Module 3: 



Motor rotation rate measurement of: 



Counter 1 (Input) 
Counter 2 (Input) 
Counter 3 (Input) 



Left screw 
Right screw 
Turbo probe 



The inputs are square wave pulse trains from each motor encoder or probe which is 
measured and converted to rotations per second using an appropriate calibration function in 
software. 



EVLAN-1 1: Ethernet & Starlan Data Link Controller 



Ethernet module for network communications between internal vehicle processors and 
external computers. An AUI (lOBaseS) to thin wire (10Base2) transceiver is attached to 
this unit. 
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GESDAC-2B: Digital/ Analog Converter. 



An 8 channel, 12 bit digital/analog converter module for input to servo amplifiers 
for control of: 



Channel 0 - 
Channel 1 - 
Channel 2 - 
Channel 3 - 
Channel 4 - 
Channel 5 - 



Left screw 
Right screw 
Bow vertical thruster 
Bow lateral thruster 
Stem vertical thruster 
Stem lateral Thruster 



GESMFI-1: Multi-Function Interface 

Contains two RS-232 serial ports and two parallel ports. 14 bits of the two parallel 
ports interface with the synchro to digital converter connected to the free gyroscope. 8 bits 
from parallel port 1 and 6 bits from port 2. One serial port is used to read position and 
message data from the vehicle DiveTracker unit. 

GESBUS-12M: Interconnection Backplane for G-64 and G-96 Euroboards 
Twelve slot Backplane for the boards listed above. 

C. ELECTRICAL COMPONENTS 
Batteries 



For the purposes of load balancing, two separate battery systems are used. Each 
system comprises of two 12 Vdc sealed lead acid batteries connected in parallel giving a 
total of 24 Vdc. One system powers the computer system, sonars, speed encoders, and 
control surface servo motors. The second system is used for the gyroscopes and 
propulsion motors. These have been separated since the computer is sensitive to voltage 
fluctuations which are caused by the propulsion motors. All batteries are rechargeable from 
an external power supply using a through-hull connection. The batteries are Panasonic 
Model LCL12V38P with nominal capacity of 38.0 Amp-hours. 
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ACON Power Supplies 



The Gespac computer system is powered by a single ACON Model R100T2405- 
12TS power supply. It provides +5, +12, and -12 Vdc and is supplied by 24 Vdc directly 
from the computer battery system. A second supply is also installed for powering other 
units such as the DiveTracker module, GPS module, and ethemet transceiver for the Sun 
Voyager. 

Calex Power Supplies 

Calex Models 12S15, 48S15, and 12S5 power supplies provide either +5, +15, or 
-15 Vdc for the following units: 

Reference source for the rate and vertical gyros ( ± 15 Vdc) 

Datasonics sonar (+15 Vdc) 

Depth cell (+15 Vdc) 

GESTIM-1 A timer cards for control surface signal channels (+15 Vdc) 

Control Surface servo motors (+5 Vdc) 

Propulsion Motor Servo Amplifiers 

Motor voltage control for the thrusters and stem screw motors is controlled through 
the use of Advanced Motion Controls PWM Model 30AD8DD servo amplifiers. One 
amplifier is used to control each motor and uses a 0 to 10 Vdc control signal to modulate 
the pulse width of a 24 Vdc, 5 to 45 kHz output signal to the motor. A control signal of 0 
Vdc provides a voltage of -24 to the motor while a 10 Vdc signal corresponds to -24 Vdc to 
be sent. 
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GESRAM 14B4 








GESMPU-30H 








GESMFI-1 








EVLAN-11 








GESSIO-IB 








GESPIA-3A 








GESADA-1 








GESADC-2C 








GESTIM-IA 








GESTIM-lA 








GESTIM-IA 








GESDAC-2B 







D. 1 Layout of Gespac Card Cage Slots 
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APPENDIX E. PROLOG PREDICATES LINKED TO 
TACTICAL LEVEL C FUNCTIONS 



The following is a listing of the C functions callable from the Strategic Level Prolog 

code. 



ood(+string, [-integer]) 

Description: This is the Officer of the Deck predicate where the value of "string" 
may be any one of the following; 

start_sun_network 

Description: Open network socket from Sun SPARC to Gespac. 

start_sun_and_iris_network 

Description: Open network socket from Sun SPARC to Gespac and IRIS Elan. 
start_networks 

Description: The same as start_sun_and_iris_network. 
start_dive_tracker 

Description: Commands the Execution Level to fork DiveTracker process. 

initialize_boards 

Description: Commands the Execution Level to initialize all I/O boards on 
Gespac. 

read_mission_file 

Description: Commands the Tactical Level to read the mission file which 
contains phase set points, time outs, etc. 

turn_on_prop_power 

Description: Commands the Execution Level to activate the thruster and rear 
screw servo amplifiers. 
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turn_on_sonar_power 

Description: Commands the Execution Level to activate the sonar power. 
turn_off_sonar_power 

Description: Commands the Execution Level to deactivate the sonar power. 
gyros_on 

Description: Tells the Execution Level that the gyroscopes are on and should be 
zeroed and read each time step. 

zero_sensors 

Description: Commands the Execution Level to zero the depth cell and 
gyroscopes at this time. 

initialize_stl000_sonar 

Description: Commands the Execution Level to initialize the STIOOO sonar 
head. 

initialize_st725_sonar 

Description: Commands the Execution Level to initialize the ST725 sonar head. 
uncage_directional_gyroscope 

Description: Commands the Execution Level to uncage the directional (free) 
gyroscope. 

iniUalization_done 

Description: Informs the Execution that no more initialization will be performed. 
shutdown_network 

Description: Causes the Tactical Level to disconnect from the Execution Level 
also from the IRIS if connected. 

engineer(+string, [-integer]) 

Description: Used to activate engineering functions. Not functional at this time. 
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Execute Predicates Always Returns 1 (TRUE) 



exec_submerge([-integer]) 

Description: Commands the vehicle to submerge using vertical thrusters. 
exec_rotate( [-integer] ) 

Description: Commands the vehicle to rotate using lateral thrusters. 
exec_servo_X([-integerJ) 

Description: Commands the vehicle to servo to wall using rear screws and sonar 
for range information. 

exec_stop_servo_X([-integer]) 

Description: Deactivates wall servoing mode. 

exec_sleep(+integer, [-integer]) 

Description: Uses the C language sleep function to halt execution for "integer" 
seconds. 

exec_start_timer([-integer]) 

Description: Starts a timer for the duration of the phase time out. Used in 
conjunction with ask_time_out. 

exec_surface([-integer]) 

Description: Commands the vehicle to surface at maximum thrust. 

exec_next_setpt_data([-integer]) 

Description: Increments the phase set point vector index. 

exec. -find. _sonar_target([-integer]) 

Description: Begin search for target of interest using object detection algorithm 
in the Sonar Manager 
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exec_start_int_control_z([-integer]) 

Description; Start implementing integral control for submergence. 

exec_start_send_stlOOO_data( [-integer]) 

Description: Activates sending of sonar data to Sonar Manager process. 

exec_stop_send_stlOOO_data([-integer]) 

Description: Deactivates sending of sonar data. 

exec_set_stlOOO_mode([-integer]) 

Description: Sets current phase ST 1000 sweep parameters. 

exec_start_ping_stlOOO_sonar([-integerJ) 

Description; Start pinging the ST 1000 sonar. 

exec_stop_ping_stl000_sonar([-integer]) 

Description: Stop pinging the ST 1000 sonar. 

exec_start_sonar_filter([-integer]) 

Description: Activates the kinematic Kalman filter using STIOOO sonar data. 

exec_set_heading([-integer]) 

Description: Used to override current heading set point. 

exec_set_heading_from_target([-integer]) 

Description: Commands the vehicle to point towards target of interest. 

exec_start_XY_psi_control([-integer]) 

Description: Commands the vehicle maneuver to set points relative to a target. 

exec_calc_pos_from_target([-integer]) 

Description: Calculates the vehicle position relative to a target. 
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exec_track_target( [-integer]) 

Description: Commands the sonar to track a target continuously. 



Query Predicates Return either a 1 (TRUE) or 0 (FALSE) 
ask_depth_reached([-integer]) 

Description: Returns TRUE if the depth is within error bounds, otherwise 
FALSE. 

ask_heading_reached([-integer]) 

Description: Returns TRUE if the heading is within error bounds, otherwise 
FALSE. 

ask_X_reached([-integer]) 

Description: Returns TRUE if the longitudinal position is within error bounds, 
otherwise FALSE. 

ask_time_out([-integer]) 

Description: Queried after a call to exec_start_timer. If the phase time-out 
has been exceeded, the predicate returns TRUE. If the elapsed time is less 
FALSE is returned. 

ask_system_problem([-integer]) 

Description: Returns TRUE if a system problem has been encountered, FALSE 
if no problems. 

ask_surface_reached( [-integer]) 

Description: Used in conjunction with exec_surface. If the depth is within .1 
feet of the surface, TRUE is returned, otherwise FALSE. 

ask_sonar_ping_out([-integer]) 

Description: Returns TRUE if allotted time to ping the sonar has expired 



325 



ask_int_control_z_on([-integer]) 

Description; Returns TRUE if integral control for submergence is active. 

ask_int_control_z_off([-integer]) 

Description: Returns TRUE if integral control for submergence is inactive. 

ask_sonar_target_found([-integer]) 

Description: Returns TRUE if target described by the Sonar Manager algorithm 
is identified. 

ask_XY_psi_reached( [-integer]) 

Description: Returns TRUE if for the particular phase is 

within error bounds. 

ask_XY_psi_control_off([-integer]) 

Description; Returns TRUE if local navigation to a target is inactive. 
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APPENDIX F. EXECUTION LEVEL C LANGUAGE 

FUNCTIONS 



The following is a listing and description of the C language Execution Level 
functions used for vehicle control and environment sensing. 



A. SENSORS 

double rolI_angle() 

Description: Returns the roll angle in radians from the vertical gyroscope, 
double pitch_angle() 

Description: Returns the pitch angle in radians from the vertical gyroscope, 
double calc_psi() 

Description: Returns the heading angle in radians from the free gyroscope. The 
angle returned includes any multiples of ±2k radians if the vehicle rotates 
beyond ± 360 ®. 

double roll_rate_gyro() 

Description: Returns the roll rate in radians/sec from the rate gyroscope, 
double pitch_rate_gyro() 

Description: Returns the pitch rate in radians/sec from the rate gyroscope, 
double yaw_rate_gyro() 

Description: Returns the yaw rate in radians/sec from the rate gyroscope, 
void read_gyros() 

Description: Performs all of the gyroscope reads listed above with a single 
function call. 
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void cage_dg() 

Description: Cages the free (directional) gyroscope, 
void uncage_dg() 

Description; Uncages the free gyroscope, 
int cageO 

Description: Returns TRUE (1) if free gyroscope is caged, and FALSE (0) if 
not. 

double depthO 

Description: Returns the vehicle depth in feet from the depth cell. 

double read_computer_battery_voltage() 

Description: Returns the computer battery voltage. 

double read_motor_gyro_battery_voItage() 

Description: Returns motor/gyro battery voltage. 

int leak_check() 

Description; Returns TRUE (1) if a leak is detected, otherwise FALSE (0). 
zero_sensors(mode) 

Description: At the time the function is called the current vehicle orientation, 
angular rate, and depth is read and these values are used as zero offsets until 
mission completion. If the gyroscopes are not to be used for a mission, the 
value mode should be set to 0, to prevent reading them. If the gyroscopes are 
to be used, mode should be set to 1. 

B . ACTUATORS 

tbruster_power(onoff) 

Description: Thruster Motor Power Control, onoff = TRUE to activate, 
FALSE to deactivate. 
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screw_power(onoff) 

Description: Rear Screw Power Control, same as above. 

motor(n, value) 

Description: Commands to motor number n voltage where value = 0-1023 
which maps to -24 - -t-24 VDC applied to the motor, 
n = 

0 Left Screw 

1 Right Screw 

2 Bow Vert. Thruster 

3 Bow Lateral Thruster 

4 Stem Vert. Thruster 

5 Stem Lateral Thmster 

zero_motors() 

Description: Sends a 0 voltage command to all motors, 
float motor_speed(n) 

Description: Returns motor speed in rotations/sec for motor n. 
ls_speed_control(n_com) 

Description: Control left rear screw to speed n_com in rotations/sec. 
rs_speed_control(n_com) 

Description; Control right rear screw to speed n_com in rotations/sec. 
rudder(angle) 

Description; Commands mdders to deflect to angle in radians. 
planes(angle) 

Description: Commands planes to deflect to angle in radians. 



329 



zero_fins() 

Description: Sends a command of zero angle to all fins, (rudders and planes). 



C. EXECUTION LEVEL PRIMITIVES 

The following are strings passed to the Execution Level from the Tactical Level. 



Initialization Primitives: 



INITIALIZE.BOARDS 

START_DIVE_TRACKER_PROCESS 

TURN_ON_PROP_POWER 

TURN_OFF_PROP_POWER 

TURN_ON_SONAR_POWER 

TURN_OFF_SONAR_POWER 

ZERO_GYROS_AND_DEPTH_CELL 

ZERO_DEPTH_CELL 

UNCAGE_DIRECTIONAL_GYROSCOPE 

INITI ALIZE_ST 1 OOO.SONAR 

INrriALIZE_ST725_SONAR 

INITIALIZATION.DONE 



Control Primitives: 
Command Strings: 



SUBMERGE 

ROTAT1E 

SERVO_X 

XY_PSI_CONTROL 

STOP_SERVO_X 

SURFACE 

START_INT_CONTROL_Z 

START_DEPTH_ERROR_FILTER 
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START_HEADING_ERROR_FILTER 

START_X_ERROR_FILTER 

RESET_HEADING_COMMAND 

SET_ST1000_MODE 
PING.STIOOO 
STOP_PING_ST1000 
START_SONAR_FILTER 
START_SEND_ST 1 000_D ATA 
STOP_SEND_ST 1 000_DATA 



Query Strings: 

GET_DEPTH_INFO 

GET_XY_PSI_INFO 

GET_ST 1 000_INrnALIZATION_STATUS 

GET_HEADING_INFO 

GET_ST725_DATA 

GET_ST1000_DATA 

GET_SERVO_X_DATA 
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APPENDIX G. STANDARD SONAR COMMANDS 



The following is a list of the standard sonar commands used for controlling the 
Tritech ST725 and ST 1000 sonars. 



Dec Chr Command 


Description 


043 ’+' MVCW 


Move 1 step ClockWise, current step size. Replies T','t','F', or 'f 


045 MVCCW 


Move 1 step CounterClockWise. Replies T','t','F’, of 'f 


065 'A' SETAV 


Set mode return range bin avg. Value. Reply is 'A' 


066 'B' SETGN 


Set gain for test purposes. Send 'B' followed by Char(gain value). 
Reply ’B' 


067 'C SETYMI 


Set initial gain for TVG (Time Varying Gain). Call is 'C followed 
by Char(Ymin) Where Ymin = 255*gain/100, 0 < gain < 100. 
Reply is 'A' 


068 'D' INDEF 


Inquire if head is using Default settings. Reply is T' if yes, 'F if 
not. 


069 'E' SETYMA 


Set final gain for TVG. Send 'E' followed 
by Char(Ymax). Reply is 'E' 


070 'F' SET96 


Set 9600 baud communication speed (Default) 


071 'G' SET 192 


Set 1 9200 baud communication speed 


072 'H' SETHS 


Set halfstep mode (0.9 deg). Reply 'H' 


073 T WRPARM 


Inquire sonar parameters. Replies: 

Word - TxPulse length in 1.96 /i sec units 
Byte - NSAMPL, No. A/D samples per bin 
Byte - NBINS, No. of bins to collect 
Byte - Range Code, 0-8 

Byte - DataByte checksum, (Lower 8 bits of the sum of above 
values) 


075 ’K’ SETPK 


Set mode return range Peak. Value (Normal Use). Reply 'K' 


076 'L' SCCW 


Scan Counterclockwise (Scan Left). Does a scan ping. Returns 
NBINS/2 bytes of data -i- TtFf , then steps CCW 


077 'M' TSTSEN 


Test head direction sensor. Replies TtFf 
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079 'O’ MOTOFF 



Switch scan motor off. Replies 'O’ 



063 '?' NA 


No Action. For comms test. Replies '?' 


080 'P' RDPARM 


Send sonar parameters. Send 'P' followed by 
Word - TxPulse length in 1.96 jUsec units 
Byte - NSAMPL, No. A/D samples per bin 
Byte - NBINS, No. of bins to collect 
Byte - Range Code, 0-8 

Byte - DataByte checksum, (Lower 8 bits of the sum of 
above values) 

Replies 'T' if checksum ok, else 'F 


082 'R' sew 


Scan Clockwise (Scan Right). Same return as SCCW 


083 'S' SCAN 


Scan but no step. Same return as SCW,SCCW 


084 'T' GetGn 


See TVG gains 


085 'U' CLRTVG 


Disable TVG, gain 0. Replies 'U' (Don’t Use) 


086 'V VER 


Returns ASCII version number '3' 


087 'W SendGn 


See TVG gains 


088 'X' SETTVG 


Enable TVG (Normal). Replies 'X' 


089 'Y' CLRHS 


Full step mode set (1.8 deg). Replies 'Y' 



Profiler Sonar Commands 



036 '$' CURDLE 


Clear DUE protocol mode. No reply 


040 '(' FESCeW 


Get echorange, step CounterClockWise (For Profiling). 
Equivalent to sending 'Z' and 
MVeeW. Replies: 

Word - Echorange 
Byte - 'TtFf 


041 ')■ FESC 


Get echorange, step ClockWise. Same reply as FESCCW 


042 '*' SETDLE 


Set DUE protocol mode. No reply 


" FHalfS 


Set step size 0.9 degrees. No reply 


049 T FFullS 


Set step size 1 .8 degrees. No reply 


050 '2' FDoubS 


Set step size 3.6 degrees. No reply 
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060 ■<’ SHORTR 


Set Profiler to 1 mm resolution. No reply 


061 ■=’ RDMOTD 


Set motor step delay time, in 1.96 /isec intervals. Send '=' 
followed by Word - Delay interval 


074 'J' RDESPA 


Send profiler parameters. Send 'J' followed by: 

Word - ECPULS - Echo TxPulse length in 1.96 fisec units 
Word - TIMOUT - Timout for max range (mm) 

Word - LOKOUT - Lockout time, min range 1.96 usee units 

Word - ESWAIT - Wait time AutoES mode 

Byte - GECMIN - Initial gain value 

Word - GAENDT - Gain increment delay for TVG 

Word - ECSCLX - Scale numerator for range 

Word - ECSCLY - Scale denominator for range 

Word - Maxdst - Max distance in range units 

Word - DACSeX - Scale numerator for DAC 0/P 

Word - DACSCY - Scale denominator for DAC 0/P 

Byte - RngUnt -0=10 mm, else 1 mm range units 

Byte - CHKSUM - DataByte checksum, (Lower 8 bits of the 

sum of above values) 

Replies T if Ok, else 'F' and returns head to defaults 


078 'N' ENQES 


Inquire if profiler support. Reply T 


081 'Q' WRESPA 


Inquire profiler parameters. Replies RDESPA ended with 
DataByte checksum 


090 'Z' ESRNG 


Profiler range. Replies: 
Word - Profiler range in mm 


091 '[’ DRON 


Profiler debug on. With 'Z' replies profiler raw time, distance 
and DAC value (0-4095) in ASCII 


093 ']' DROFF 


Profiler debug off 
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APPENDIX H. POLYGON INTERSECTION ALGORITHM 



The intersection of a straight line with a plane was taken from (Dewey, 1988). A 
line may be represented by the equation: 

P(t) = P(0) + cet (0 < t < 1) (H.l) 

where P( 0) is the point which locates the start of the line, e is a unit vector in the direction 
of the line, and c is a constant which specifies the length of the line (Figure H.l). The 
equation of the plane is 



P(u,v) = P(0,0) + CjCjU + c,ejV (H.2) 

where u and v have the range 0 to 1 and c, and are the length and width dimensions of 
the plane of interest, and are orthogonal unit vectors which lie on the plane. The 
system of equations which describe the point of intersection is 

P^ = P(0,0) + CjejU + CjCjV = P(0) + cet (H.3) 



The three unknowns u, v, and t are uniquely determined by the three equations. 
The above equations may be expanded in matrix form to yield 



where 
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(H.4) 
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Equation (4) may be written in compact form as 



Mu = z 



(H.5) 



and the solution vector u is obtained by 



u 



r \ 

U 

1 ^ ' 
t 



M~'z 



(H.6) 



The point of intersection is simply 



= p(0) + cet (H.7) 

If any of the solutions to m, v, or r is out of the range 0 to 1, the intersection lies on the 
extended plane (i.e. outside of the plane area bounded by c, and c,). If the line is parallel 

to the plane, M is singular and no solution exists. 
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RECTANGULAR PLANE 




H.l Vector Definitions for the Polygon Intersection Algorithm. 
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